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

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

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

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

177 files changed, 17657 insertions, 23311 deletions

diff --git a/Android.mk b/Android.mk
new file mode 100644
index 0000000..5053e7d
--- /dev/null
+++ b/Android.mk
@@ -0,0 +1 @@
+include $(call all-subdir-makefiles)
diff --git a/libsensors/Android.mk b/libsensors/Android.mk
index 06c01e3..f6c9323 100644
--- a/libsensors/Android.mk
+++ b/libsensors/Android.mk
@@ -13,10 +13,9 @@
 # limitations under the License.
 # Modified 2011 by InvenSense, Inc
 
-
 LOCAL_PATH := $(call my-dir)
 
-ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
+ifneq ($(TARGET_SIMULATOR),true)
 
 # InvenSense fragment of the HAL
 include $(CLEAR_VARS)
@@ -26,23 +25,120 @@ LOCAL_MODULE := libinvensense_hal
 LOCAL_MODULE_TAGS := optional
 
 LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1
 
-LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp 
+ifeq ($(ENG_BUILD),1)
+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
+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
+
+LOCAL_SRC_FILES := SensorBase.cpp
+LOCAL_SRC_FILES += MPLSensor.cpp
+LOCAL_SRC_FILES += MPLSupport.cpp
+LOCAL_SRC_FILES += InputEventReader.cpp
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
 
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include/linux
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/linux
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mllite
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mldmp
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/aichi
-LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/akmd
+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
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
 
-LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
-LOCAL_CPPFLAGS+=-DLINUX=1
-LOCAL_LDFLAGS:=-rdynamic
+LOCAL_SHARED_LIBRARIES := liblog
+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
+LOCAL_CPPFLAGS += -DLINUX=1
 LOCAL_PRELINK_MODULE := false
 
 include $(BUILD_SHARED_LIBRARY)
 
+ifeq ($(BUILD_FAKE_SENSORS_MANTA),1)
+# Build a temporary HAL that links the InvenSense .so
+include $(CLEAR_VARS)
+# LOCAL_MODULE := sensors.$(TARGET_PRODUCT)
+LOCAL_MODULE := sensors.manta
+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 ($(ENG_BUILD),1)
+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
+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 # ENG_BUID
+
+LOCAL_SRC_FILES := sensors_mpl.cpp
+
+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)
+endif
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmplmpu
+LOCAL_SRC_FILES := libmplmpu.so
+LOCAL_MODULE_TAGS := optional
+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)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_SRC_FILES := libmllite.so
+LOCAL_MODULE_TAGS := optional
+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/CompassSensor.IIO.9150.cpp b/libsensors/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..ce0df34
--- /dev/null
+++ b/libsensors/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,421 @@
+/*
+ * 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.9150.h"
+#include "sensors.h"
+#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"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_YAS530"
+#elif 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
+#   warning "Invensense compass cal with compass on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_COMPASS"
+#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(NULL, NULL),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8),
+                    compass_fd(-1)
+{
+    VFUNC_LOG;
+
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(COMPASS_NAME, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    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", 
+               &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");
+    }
+
+    enable(ID_M, 0);
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+}
+
+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;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+            en, compassSysFs.compass_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_enable, O_RDWR);
+    res = errno;
+    if(tempFd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_enable, strerror(res), res);
+        return res;
+    }
+    res = enable_sysfs_sensor(tempFd, en);
+    LOGE_IF(res < 0, "HAL:enable compass failed");
+    close(tempFd);
+
+    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);
+            close(tempFd);
+        } 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);
+            close(tempFd);
+        } 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);
+            close(tempFd);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_z_fifo_enable, strerror(res), res);
+        }
+    }
+
+    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)
+{
+    VHANDLER_LOG;
+
+    int numEventReceived = 0, done = 0;
+
+    ssize_t n = mCompassInputReader.fill(compass_fd);
+    if (n < 0) {
+        LOGE("HAL:no compass events read");
+        return n;
+    }
+
+    input_event const* event;
+
+    while (done == 0 && mCompassInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_REL) {
+            processCompassEvent(event);
+        } else if (type == EV_SYN) {
+            *timestamp = mCompassTimestamp;
+            memcpy(data, mCachedCompassData, sizeof(mCachedCompassData));
+            done = 1;
+        } else {
+            LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)",
+                 type, event->code);
+        }
+        mCompassInputReader.next();
+    }
+
+    return done;
+}
+
+/**
+ *  @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 = COMPASS_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")
+                || !strcmp(compass, "INV_YAS530")) {
+            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_AMI306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_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;
+}
+
+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;
+
+    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 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);
+
+#if defined COMPASS_YAS530 || 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");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+    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");
+#endif
+
+#if 0
+    // test print sysfs paths   
+    dptr = (char**)&compassSysFs;
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/libsensors/CompassSensor.IIO.9150.h b/libsensors/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..6a51338
--- /dev/null
+++ b/libsensors/CompassSensor.IIO.9150.h
@@ -0,0 +1,93 @@
+/*
+ * 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"
+
+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); }
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       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;
+    } compassSysFs;
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int compass_fd;
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int64_t mDelay;
+    int mEnable;
+    char *pathP;
+
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/libsensors/InputEventReader.cpp b/libsensors/InputEventReader.cpp
new file mode 100644
index 0000000..ca0a61a
--- /dev/null
+++ b/libsensors/InputEventReader.cpp
@@ -0,0 +1,105 @@
+/*

+* 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;

+    }

+}

diff --git a/libsensors/InputEventReader.h b/libsensors/InputEventReader.h
new file mode 100644
index 0000000..11c4e73
--- /dev/null
+++ b/libsensors/InputEventReader.h
@@ -0,0 +1,47 @@
+/*

+* 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_INPUT_EVENT_READER_H
+#define ANDROID_INPUT_EVENT_READER_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+/*****************************************************************************/
+
+struct input_event;
+
+class InputEventCircularReader
+{
+    struct input_event* const mBuffer;
+    struct input_event* const mBufferEnd;
+    struct input_event* mHead;
+    struct input_event* mCurr;
+    ssize_t mFreeSpace;
+
+public:
+    InputEventCircularReader(size_t numEvents);
+    ~InputEventCircularReader();
+    ssize_t fill(int fd);
+    ssize_t readEvent(input_event const** events);
+    void next();
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_INPUT_EVENT_READER_H
diff --git a/libsensors/License.txt b/libsensors/License.txt
new file mode 100644
index 0000000..930f931
--- /dev/null
+++ b/libsensors/License.txt
@@ -0,0 +1,217 @@
+SOFTWARE LICENSE AGREEMENT
+
+Unless you and InvenSense Corporation ("InvenSense") execute a separate written
+software license agreement governing use of the accompanying software, this
+software is licensed to you under the terms of this Software License
+Agreement ("Agreement").
+
+ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR
+ACCEPTANCE OF THIS AGREEMENT.
+
+1.  DEFINITIONS.
+
+1.1.    "InvenSense Product" means any of the proprietary integrated circuit
+product(s) sold by InvenSense with which the Software was designed to be used,
+or their successors.
+
+1.2.    "Licensee" means you or if you are accepting on behalf of an entity
+then the entity and its affiliates exercising rights under, and complying
+with all of the terms of this Agreement.
+
+1.3.    "Software" shall mean that software made available by InvenSense to
+Licensee in binary code form with this Agreement.
+
+2.  LICENSE GRANT; OWNERSHIP
+
+2.1.    License Grants.  Subject to the terms and conditions of this Agreement,
+InvenSense hereby grants to Licensee a non-exclusive, non-transferable,
+royalty-free license (i) to use and integrate the Software in conjunction
+with any other software; and (ii) to reproduce and distribute the Software
+complete, unmodified and only for use with a InvenSense Product.
+
+2.2.    Restriction on Modification.  If and to the extent that the Software is
+designed to be compliant with any published communications standard
+(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards),
+Licensee may not make any modifications to the Software that would cause the
+Software or the accompanying InvenSense Products to be incompatible with such
+standard.
+
+2.3.    Restriction on Distribution.  Licensee shall only distribute the
+Software (a) under the terms of this Agreement and a copy of this Agreement
+accompanies such distribution, and (b) agrees to defend and indemnify
+InvenSense and its licensors from and against any damages, costs, liabilities,
+settlement amounts and/or expenses (including attorneys' fees) incurred in
+connection with any claim, lawsuit or action by any third party that arises
+or results from the use or distribution of any and all Software by the
+Licensee except as contemplated herein.
+
+2.4.    Proprietary Notices.  Licensee shall not remove, efface or obscure any
+copyright or trademark notices from the Software.  Licensee shall include
+reproductions of the InvenSense copyright notice with each copy of the
+Software, except where such Software is embedded in a manner not readily
+accessible to the end user.  Licensee acknowledges that any symbols,
+trademarks, tradenames, and service marks adopted by InvenSense to identify the
+Software belong to InvenSense and that Licensee shall have no rights therein.
+
+2.5.    Ownership.  InvenSense shall retain all right, title and interest,
+including all intellectual property rights, in and to the Software.  Licensee
+hereby covenants that it will not assert any claim that the Software created
+by or for InvenSense infringe any intellectual property right owned or
+controlled by Licensee.
+
+2.6.    No Other Rights Granted; Restrictions.  Apart from the license rights
+expressly set forth in this Agreement, InvenSense does not grant and Licensee
+does not receive any ownership right, title or interest nor any security
+interest or other interest in any intellectual property rights relating to
+the Software, nor in any copy of any part of the foregoing.  No license is
+granted to Licensee in any human readable code of the Software (source code).
+Licensee shall not (i) use, license, sell or otherwise distribute the
+Software except as provided in this Agreement, (ii) attempt to reverse
+engineer, decompile or disassemble any portion of the Software; or (iii) use
+the Software or other material in violation of any applicable law or
+regulation, including but not limited to any regulatory agency, such as FCC,
+rules.
+
+3.  NO WARRANTY OR SUPPORT
+
+3.1.    No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND
+LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE,
+COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE.  INVENSENSE SPECIFICALLY
+DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC
+PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR
+DOCUMENTATION FOR THE SOFTWARE.  WITHOUT LIMITATION OF THE ABOVE, INVENSENSE
+GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT
+INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS
+THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR
+RELIABILITY.
+
+3.2.    No Support.  Nothing in this agreement shall obligate InvenSense to
+provide any support for the Software. InvenSense may, but shall be under no
+obligation to, correct any defects in the Software and/or provide updates to
+licensees of the Software.  Licensee shall make reasonable efforts to
+promptly report to InvenSense any defects it finds in the Software, as an aid
+to creating improved revisions of the Software.
+
+3.3.    Dangerous Applications.  The Software is not designed, intended, or
+certified for use in components of systems intended for the operation of
+weapons, weapons systems, nuclear installations, means of mass
+transportation, aviation, life-support computers or equipment (including
+resuscitation equipment and surgical implants), pollution control, hazardous
+substances management, or for any other dangerous application in which the
+failure of the Software could create a situation where personal injury or
+death may occur.  Licensee understands that use of the Software in such
+applications is fully at the risk of Licensee.
+
+4.  TERM AND TERMINATION
+
+4.1.    Termination.  This Agreement will automatically terminate if Licensee
+fails to comply with any of the terms and conditions hereof. In such event,
+Licensee must destroy all copies of the Software and all of its component
+parts.
+
+4.2.    Effect Of Termination.  Upon any termination of this Agreement, the
+rights and licenses granted to Licensee under this Agreement shall
+immediately terminate.
+
+4.3.    Survival.  The rights and obligations under this Agreement which by
+their nature should survive termination will remain in effect after
+expiration or termination of this Agreement.
+
+5.  CONFIDENTIALITY
+
+5.1.    Obligations.  Licensee acknowledges and agrees that any documentation
+relating to the Software, and any other information (if such other
+information is identified as confidential or should be recognized as
+confidential under the circumstances) provided to Licensee by InvenSense
+hereunder (collectively, "Confidential Information") constitute the
+confidential and proprietary information of InvenSense, and that Licensee's
+protection thereof is an essential condition to Licensee's use and possession
+of the Software.  Licensee shall retain all Confidential Information in
+strict confidence and not disclose it to any third party or use it in any way
+except under a written agreement with terms and conditions at least as
+protective as the terms of this Section.  Licensee will exercise at least the
+same amount of diligence in preserving the secrecy of the Confidential
+Information as it uses in preserving the secrecy of its own most valuable
+confidential information, but in no event less than reasonable diligence.
+Information shall not be considered Confidential Information if and to the
+extent that it: (i) was in the public domain at the time it was disclosed or
+has entered the public domain through no fault of Licensee; (ii) was known to
+Licensee, without restriction, at the time of disclosure as proven by the
+files of Licensee in existence at the time of disclosure; or (iii) becomes
+known to Licensee, without restriction, from a source other than InvenSense
+without breach of this Agreement by Licensee and otherwise not in violation
+of InvenSense's rights.
+
+5.2.    Return of Confidential Information.  Notwithstanding the foregoing, all
+documents and other tangible objects containing or representing InvenSense
+Confidential Information and all copies thereof which are in the possession
+of Licensee shall be and remain the property of InvenSense, and shall be
+promptly returned to InvenSense upon written request by InvenSense or upon
+termination of this Agreement.
+
+6.  LIMITATION OF LIABILITY
+TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF
+INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL,
+SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR
+OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS
+OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT
+(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR
+SOFTWARE UNDER THIS AGREEMENT.  THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING
+ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY.
+
+7.  MISCELLANEOUS
+
+7.1.    Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS
+SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND
+REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE
+OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS.
+WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE
+TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED.
+
+7.2 Assignment. This Agreement shall be binding upon and inure to the
+benefit of the parties and their respective successors and assigns, provided,
+however that Licensee may not assign this Agreement or any rights or
+obligation hereunder, directly or indirectly, by operation of law or
+otherwise, without the prior written consent of InvenSense, and any such
+attempted assignment shall be void.  Notwithstanding the foregoing, Licensee
+may assign this Agreement to a successor to all or substantially all of its
+business or assets to which this Agreement relates that is not a competitor
+of InvenSense.
+
+7.3.    Governing Law; Venue.  This Agreement shall be governed by the laws of
+California without regard to any conflict-of-laws rules, and the United
+Nations Convention on Contracts for the International Sale of Goods is hereby
+excluded.  The sole jurisdiction and venue for actions related to the subject
+matter hereof shall be the state and federal courts located in the County of
+Orange, California, and both parties hereby consent to such jurisdiction and
+venue.
+
+7.4.    Severability.  All terms and provisions of this Agreement shall, if
+possible, be construed in a manner which makes them valid, but in the event
+any term or provision of this Agreement is found by a court of competent
+jurisdiction to be illegal or unenforceable, the validity or enforceability
+of the remainder of this Agreement shall not be affected if the illegal or
+unenforceable provision does not materially affect the intent of this
+Agreement.  If the illegal or unenforceable provision materially affects the
+intent of the parties to this Agreement, this Agreement shall become
+terminated.
+
+7.5.    Equitable Relief.  Licensee hereby acknowledges that its breach of this
+Agreement would cause irreparable harm and significant injury to InvenSense
+that may be difficult to ascertain and that a remedy at law would be
+inadequate.  Accordingly, Licensee agrees that InvenSense shall have the right
+to seek and obtain immediate injunctive relief to enforce obligations under
+the Agreement in addition to any other rights and remedies it may have.
+
+7.6.    Waiver.  The waiver of, or failure to enforce, any breach or default
+hereunder shall not constitute the waiver of any other or subsequent breach
+or default.
+
+7.7.    Entire Agreement.  This Agreement sets forth the entire Agreement
+between the parties and supersedes any and all prior proposals, agreements
+and representations between them, whether written or oral concerning the
+Software.  This Agreement may be changed only by mutual agreement of the
+parties in writing.
+
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp
index a36a565..ae82459 100644
--- a/libsensors/MPLSensor.cpp
+++ b/libsensors/MPLSensor.cpp
@@ -1,21 +1,21 @@
 /*
- * Copyright (C) 2011 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
-//see also the EXTRA_VERBOSE define, below
+* 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
+//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
 
 #include <fcntl.h>
 #include <errno.h>
@@ -26,195 +26,221 @@
 #include <dirent.h>
 #include <stdlib.h>
 #include <sys/select.h>
+#include <sys/syscall.h>
 #include <dlfcn.h>
 #include <pthread.h>
-
 #include <cutils/log.h>
 #include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include <string.h>
+#include <linux/input.h>
 
 #include "MPLSensor.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
 
-#include "math.h"
-#include "ml.h"
-#include "mlFIFO.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "ml_mputest.h"
+#include "invensense.h"
+#include "invensense_adv.h"
 #include "ml_stored_data.h"
-#include "mldl_cfg.h"
-#include "mldl.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
 
-#include "mpu.h"
-#include "accel.h"
-#include "compass.h"
-#include "kernel/timerirq.h"
-#include "kernel/mpuirq.h"
-#include "kernel/slaveirq.h"
+// #define TESTING
+// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
 
-extern "C" {
-#include "mlsupervisor.h"
-}
+#ifdef THIRD_PARTY_ACCEL
+#   warning "Third party accel"
+#   define USE_THIRD_PARTY_ACCEL        (1)
+#else
+#   define USE_THIRD_PARTY_ACCEL        (0)
+#endif
 
-#include "mlcontrol.h"
-#include "sensor_params.h"
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+/******************************************************************************/
+/*  MPL interface misc.                                                       */
+/******************************************************************************/
+static int hertz_request = 200;
+
+#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
+#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
 
-#define EXTRA_VERBOSE (0)
-//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__)
-#define FUNC_LOG
-#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__)
-/* this mask must turn on only the sensors that are present and managed by the MPL */
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO)
+#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
+#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
+#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
+#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
 
-#define CALL_MEMBER_FN(pobject,ptrToMember)  ((pobject)->*(ptrToMember))
+/* convert ns to hardware units */
+#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
+#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
+#define HW_COMPASS_RATE_NS              (rate_request)                // to ns
 
-/******************************************/
+/* convert Hz to hardware units */
+#define HW_GYRO_RATE_HZ                 (hertz_request)
+#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
+#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
 
-/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */
 static struct sensor_t sSensorList[] =
-    { { "MPL Gyroscope", "Invensense", 1,
-         SENSORS_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, { } },
-      { "MPL Magnetic Field", "Invensense", 1,
-         SENSORS_MAGNETIC_FIELD_HANDLE,
-         SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, { } },
-      { "MPL Orientation", "Invensense", 1,
-         SENSORS_ORIENTATION_HANDLE,
-         SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, { } },
-      { "MPL Rotation Vector", "Invensense", 1,
-         SENSORS_ROTATION_VECTOR_HANDLE,
-         SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, { } },
-      { "MPL Linear Acceleration", "Invensense", 1,
-         SENSORS_LINEAR_ACCEL_HANDLE,
-         SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, { } },
-      { "MPL Gravity", "Invensense", 1,
-         SENSORS_GRAVITY_HANDLE,
-         SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, { } },
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_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, {}},
+    {"MPL Magnetic Field", "Invensense", 1,
+     SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Orientation", "Invensense", 1,
+     SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, {}},
+    {"MPL Rotation Vector", "Invensense", 1,
+     SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Linear Acceleration", "Invensense", 1,
+     SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Gravity", "Invensense", 1,
+     SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
 };
 
-static unsigned long long irq_timestamp = 0;
-/* ***************************************************************************
- * MPL interface misc.
- */
-//static pointer to the object that will handle callbacks
-static MPLSensor* gMPLSensor = NULL;
+MPLSensor *MPLSensor::gMPLSensor = NULL;
 
-/* we need to pass some callbacks to the MPL.  The mpl is a C library, so
- * wrappers for the C++ callback implementations are required.
- */
 extern "C" {
-//callback wrappers go here
-void mot_cb_wrapper(uint16_t val)
-{
-    if (gMPLSensor) {
-        gMPLSensor->cbOnMotion(val);
-    }
-}
-
 void procData_cb_wrapper()
 {
-    if(gMPLSensor) {
-        gMPLSensor->cbProcData();
+    if(MPLSensor::gMPLSensor) {
+        MPLSensor::gMPLSensor->cbProcData();
     }
 }
 
-} //end of extern C
-
 void setCallbackObject(MPLSensor* gbpt)
 {
-    gMPLSensor = gbpt;
+    MPLSensor::gMPLSensor = gbpt;
 }
 
+MPLSensor* getCallbackObject() {
+    return MPLSensor::gMPLSensor;
+}
 
-/*****************************************************************************
- * sensor class implementation
- */
+} // end of extern C
+
+#ifdef INV_PLAYBACK_DBG
+static FILE *logfile = NULL;
+#endif
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+MPLSensor::MPLSensor(CompassSensor *compass)
+                       : SensorBase(NULL, NULL),
+                         mNewData(0),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(ALL_MPL_SENSORS_NP),
+                         mPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mSampleCount(0),
+                         mEnabled(0),
+                         mOldEnabledMask(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mGyroOrientation{0},
+                         mAccelOrientation{0},
+                         mFeatureActiveMask(0) {
+    VFUNC_LOG;
 
-#define GY_ENABLED ((1<<ID_GY) & enabled_sensors)
-#define A_ENABLED  ((1<<ID_A)  & enabled_sensors)
-#define O_ENABLED  ((1<<ID_O)  & enabled_sensors)
-#define M_ENABLED  ((1<<ID_M)  & 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)
-
-MPLSensor::MPLSensor() :
-    SensorBase(NULL, NULL),
-            mMpuAccuracy(0), mNewData(0),
-            mDmpStarted(false),
-            mMasterSensorMask(INV_ALL_SENSORS),
-            mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
-            mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
-            mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
-            mUseTimerirq(false), mSampleCount(0),
-            mEnabled(0), mPendingMask(0)
-{
-    FUNC_LOG;
     inv_error_t rv;
-    int mpu_int_fd, i;
+    int i, fd;
     char *port = NULL;
+    char *ver_str;
+    unsigned long mSensorMask;
+    int res;
+    FILE *fptr;
 
-    ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
+    mCompassSensor = compass;
+
+    LOGV_IF(EXTRA_VERBOSE,
+            "HAL:MPLSensor constructor : numSensors = %d", numSensors);
 
     pthread_mutex_init(&mMplMutex, NULL);
+    pthread_mutex_init(&mHALMutex, NULL);
 
-    mForceSleep = false;
+#ifdef INV_PLAYBACK_DBG
+    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
+    logfile = fopen("/data/playback.bin", "wb");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
 
-    /* used for identifying whether 9axis is enabled or not             */
-    /* this variable will be changed in initMPL() when libmpl is loaded */
-    /* sensor list will be changed based on this variable               */
-    mNineAxisEnabled = false;
+    /* setup sysfs paths */
+    inv_init_sysfs_attributes();
 
-    for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
-        mPollFds[i].fd = -1;
-        mPollFds[i].events = 0;
+    /* get chip name */
+    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
+        LOGE("HAL:ERR- Failed to get chip ID\n");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
     }
 
-    pthread_mutex_lock(&mMplMutex);
+    enable_iio_sysfs();
 
-    mpu_int_fd = open("/dev/mpuirq", O_RDWR);
-    if (mpu_int_fd == -1) {
-        ALOGE("could not open the mpu irq device node");
-    } else {
-        fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
-        //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0);
-        mIrqFds.add(MPUIRQ_FD, mpu_int_fd);
-        mPollFds[MPUIRQ_FD].fd = mpu_int_fd;
-        mPollFds[MPUIRQ_FD].events = POLLIN;
-    }
+    /* turn on power state */
+    onPower(1);
 
-    accel_fd = open("/dev/accelirq", O_RDWR);
-    if (accel_fd == -1) {
-        ALOGE("could not open the accel irq device node");
-    } else {
-        fcntl(accel_fd, F_SETFL, O_NONBLOCK);
-        //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0);
-        mIrqFds.add(ACCELIRQ_FD, accel_fd);
-        mPollFds[ACCELIRQ_FD].fd = accel_fd;
-        mPollFds[ACCELIRQ_FD].events = POLLIN;
-    }
+    /* reset driver master enable */
+    masterEnable(0);
 
-    timer_fd = open("/dev/timerirq", O_RDWR);
-    if (timer_fd == -1) {
-        ALOGE("could not open the timer irq device node");
+    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+    // TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    loadDMP();	
+#endif
+
+    /* open temperature fd for temp comp */
+    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
+    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
+    if (gyro_temperature_fd == -1) {
+        LOGE("HAL:could not open temperature node");
     } else {
-        fcntl(timer_fd, F_SETFL, O_NONBLOCK);
-        //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0);
-        mIrqFds.add(TIMERIRQ_FD, timer_fd);
-        mPollFds[TIMERIRQ_FD].fd = timer_fd;
-        mPollFds[TIMERIRQ_FD].events = POLLIN;
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:temperature_fd opened: %s", mpu.temperature);
     }
 
-    data_fd = mpu_int_fd;
+    /* read accel FSR to calcuate accel scale later */
+    if (USE_THIRD_PARTY_ACCEL == 0) {
+        char buf[3];
+        int count = 0;
+        LOGV_IF(SYSFS_VERBOSE, 
+                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
 
-    if ((accel_fd == -1) && (timer_fd != -1)) {
-        //no accel irq and timer available
-        mUseTimerIrqAccel = true;
-        //ALOGD("MPLSensor falling back to timerirq for accel data");
+        fd = open(mpu.accel_fsr, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening accel FSR");
+        } else {
+           memset(buf, 0, sizeof(buf));
+           count = read_attribute_sensor(fd, buf, sizeof(buf));
+           if(count < 1) {
+               LOGE("HAL:Error reading accel FSR");
+           } else {
+               count = sscanf(buf, "%d", &mAccelScale);
+               if(count)
+                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
+           }
+           close(fd);
+        }
     }
 
+    /* initialize sensor data */
     memset(mPendingEvents, 0, sizeof(mPendingEvents));
 
     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
@@ -245,10 +271,12 @@ MPLSensor::MPLSensor() :
     mPendingEvents[Accelerometer].acceleration.status
             = SENSOR_STATUS_ACCURACY_HIGH;
 
+    /* Invensense compass calibration */
     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
     mPendingEvents[MagneticField].sensor = ID_M;
     mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
-    mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
+    mPendingEvents[MagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Orientation].version = sizeof(sensors_event_t);
     mPendingEvents[Orientation].sensor = ID_O;
@@ -264,579 +292,1059 @@ MPLSensor::MPLSensor() :
     mHandlers[MagneticField] = &MPLSensor::compassHandler;
     mHandlers[Orientation] = &MPLSensor::orienHandler;
 
-    for (int i = 0; i < numSensors; i++)
-        mDelays[i] = 30000000LLU; // 30 ms by default
-
-    if (inv_serial_start(port) != INV_SUCCESS) {
-        ALOGE("Fatal Error : could not open MPL serial interface");
+    for (int i = 0; i < numSensors; i++) {
+        mDelays[i] = 0;
     }
 
-    //initialize library parameters
-    initMPL();
+    (void)inv_get_version(&ver_str);
+    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
 
-    //setup the FIFO contents
-    setupFIFO();
+    /* setup MPL */
+    inv_constructor_init();
 
-    //we start the motion processing only when a sensor is enabled...
-    //rv = inv_dmp_start();
-    //ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv);
-    //dmp_started = true;
+    /* load calibration file from /data/cal.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);
+
+    inv_set_device_properties();
+
+    /* disable driver master enable the first sensor goes on */
+    masterEnable(0);
+    enableGyro(0);
+    enableAccel(0);
+    enableCompass(0);
+    onPower(0);
+
+#ifdef INV_PLAYBACK_DBG
+    logfile = fopen("/data/playback.bin", "wb");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+}
 
-    pthread_mutex_unlock(&mMplMutex);
 
-}
+void MPLSensor::enable_iio_sysfs() {
+    VFUNC_LOG;
 
-MPLSensor::~MPLSensor()
-{
-    FUNC_LOG;
-    pthread_mutex_lock(&mMplMutex);
-    if (inv_dmp_stop() != INV_SUCCESS) {
-        ALOGW("Error: could not stop the DMP correctly.\n");
+    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)", 
+            mpu.in_timestamp_en, getTimestamp());
+    tempFd = open(mpu.in_timestamp_en, O_RDWR);
+    if(tempFd < 0) {
+        LOGE("HAL:could not open timestamp enable");
+    } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
+        LOGE("HAL:could not enable timestamp");
     }
 
-    if (inv_dmp_close() != INV_SUCCESS) {
-        ALOGW("Error: could not close the DMP");
+    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");
+    }
+    fclose(tempFp);
+
+    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");
+    }
+    fclose(tempFp);
+
+    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");
     }
+    fclose(tempFp);
 
-    if (inv_serial_stop() != INV_SUCCESS) {
-        ALOGW("Error : could not close the serial port");
+    inv_get_iio_device_node(iio_device_node);
+    iio_fd = open(iio_device_node, O_RDONLY);
+    if (iio_fd < 0) {
+        LOGE("HAL:could not open iio device node");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
     }
-    pthread_mutex_unlock(&mMplMutex);
-    pthread_mutex_destroy(&mMplMutex);
 }
 
-/* clear any data from our various filehandles */
-void MPLSensor::clearIrqData(bool* irq_set)
+int MPLSensor::inv_constructor_init()
 {
-    unsigned int i;
-    int nread;
-    struct mpuirq_data irqdata;
-
-    poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared
-
-    for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
-        int cur_fd = mPollFds[i].fd;
-        int j = 0;
-        if (mPollFds[i].revents & POLLIN) {
-            nread = read(cur_fd, &irqdata, sizeof(irqdata));
-            if (nread > 0) {
-                irq_set[i] = true;
-                irq_timestamp = irqdata.irqtime;
-                //ALOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++);
-            }
-        }
-        mPollFds[i].revents = 0;
+    VFUNC_LOG;
+
+    inv_error_t result = inv_init_mpl();
+    if (result) {
+        LOGE("HAL:inv_init_mpl() failed");
+        return result;
+    }
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOGE("HAL:inv_start_mpl() failed");
+        LOG_RESULT_LOCATION(result);
+        return result;
     }
+
+    return result;
 }
 
-/* set the power states of the various sensors based on the bits set in the
- * enabled_sensors parameter.
- * this function modifies globalish state variables.  It must be called with the mMplMutex held. */
-void MPLSensor::setPowerStates(int enabled_sensors)
+int MPLSensor::inv_constructor_default_enable()
 {
-    FUNC_LOG;
-    bool irq_set[5] = { false, false, false, false, false };
+    VFUNC_LOG;
 
-    //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted);
+    inv_error_t result;
 
-    do {
+    result = inv_enable_quaternion();
+    if (result) {
+        LOGE("HAL:Cannot enable quaternion\n");
+        return result;
+    }
 
-        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
-            mLocalSensorMask = ALL_MPL_SENSORS_NP;
-            break;
-        }
+    result = inv_enable_in_use_auto_calibration();
+    if (result) {
+        return result;
+    }
 
-        if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
-            mLocalSensorMask = 0;
-            break;
+    // TODO: double-check for GED tablet
+    // result = inv_enable_motion_no_motion();
+    result = inv_enable_fast_nomot();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_gyro_tc();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_hal_outputs();
+    if (result) {
+        return result;
+    }
+
+    if (!mCompassSensor->providesCalibration()) {
+        /* Invensense compass calibration */
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
+        result = inv_enable_vector_compass_cal();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
         }
 
-        if (GY_ENABLED) {
-            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
-        } else {
-            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        result = inv_enable_compass_bias_w_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
         }
 
-        if (A_ENABLED) {
-            mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
-        } else {
-            mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
+        result = inv_enable_heading_from_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
         }
 
-        if (M_ENABLED) {
-            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
-        } else {
-            mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
+        result = inv_enable_magnetic_disturbance();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
         }
+    }
 
-    } while (0);
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
 
-    //record the new sensor state
-    inv_error_t rv;
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
 
-    long sen_mask = mLocalSensorMask & mMasterSensorMask;
+    // TODO: double-check for GED tablet
+    result = inv_enable_quat_accuracy_monitor();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
 
-    bool changing_sensors = ((inv_get_dl_config()->requested_sensors
-            != sen_mask) && (sen_mask != 0));
-    bool restart = (!mDmpStarted) && (sen_mask != 0);
+    return result;
+}
 
-    if (changing_sensors || restart) {
+/* TODO: create function pointers to calculate scale */
+void MPLSensor::inv_set_device_properties()
+{
+    VFUNC_LOG;
 
-        ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);
+    unsigned short orient;
 
-        if (mDmpStarted) {
-            inv_dmp_stop();
-            clearIrqData(irq_set);
-            mDmpStarted = false;
-        }
+    inv_get_sensors_orientation();
 
-        if (sen_mask != inv_get_dl_config()->requested_sensors) {
-            //ALOGV("setPowerStates: %lx", sen_mask);
-            rv = inv_set_mpu_sensors(sen_mask);
-            ALOGE_IF(rv != INV_SUCCESS,
-                    "error: unable to set MPL sensor power states (sens=%ld retcode = %d)",
-                    sen_mask, rv);
-        }
+    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
+    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
 
-        if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS))
-                || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL)))
-                && ((sen_mask & INV_DMP_PROCESSOR) == 0)) {
-            ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
-            mUseTimerirq = true;
-        } else {
-            if (mUseTimerirq) {
-                ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
-                clearIrqData(irq_set);
-            }
-            ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
-            mUseTimerirq = false;
-        }
+    /* gyro setup */
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
+    inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
 
-        if (!mDmpStarted) {
-            if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
-                rv = inv_store_calibration();
-                ALOGE_IF(rv != INV_SUCCESS,
-                        "error: unable to store MPL calibration file");
-                mHaveGoodMpuCal = false;
-                mHaveGoodCompassCal = false;
-            }
-            //ALOGV("Starting DMP");
-            rv = inv_dmp_start();
-            ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
-            mDmpStarted = true;
-        }
+    /* accel setup */
+    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
+    // BMA250
+    //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    // MPU6050
+    inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
+
+    /* compass setup */
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient = 
+        inv_orientation_matrix_to_scalar(orientMtx);
+    long sensitivity;
+    sensitivity = mCompassSensor->getSensitivity();
+    inv_set_compass_orientation_and_scale(orient, sensitivity);
+}
+
+void MPLSensor::loadDMP()
+{
+    int res, fd;
+    FILE *fptr;
+
+    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
+        //DMP support only for MPU6xxx/9xxx currently
+        return;
     }
 
-    //check if we should stop the DMP
-    if (mDmpStarted && (sen_mask == 0)) {
-        //ALOGV("Stopping DMP");
-        rv = inv_dmp_stop();
-        ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)",
-                rv);
-        if (mUseTimerirq) {
-            ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
+    /* load DMP firmware */
+    LOGV_IF(SYSFS_VERBOSE, 
+            "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
+    fd = open(mpu.firmware_loaded, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:could not open dmp state");
+    } else {
+        if(inv_read_dmp_state(fd) == 0) {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+            fptr = fopen(mpu.dmp_firmware, "w");
+            if(!fptr) {
+                LOGE("HAL:could not write to dmp");
+            } else {
+                int res = inv_load_dmp(fptr);
+                if(res < 0) {
+                    LOGE("HAL:load DMP failed");
+                } else {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+                }
+                fclose(fptr);
+            }
+        } else {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
         }
-        clearIrqData(irq_set);
-
-        mDmpStarted = false;
-        mPollTime = -1;
-        mCurFifoRate = -1;
     }
 
+    // onDMP(1);		//Can't enable here. See note onDMP()
 }
 
-/**
- * container function for all the calls we make once to set up the MPL.
- */
-void MPLSensor::initMPL()
+void MPLSensor::inv_get_sensors_orientation()
 {
-    FUNC_LOG;
-    inv_error_t result;
-    unsigned short bias_update_mask = 0xFFFF;
-    struct mldl_cfg *mldl_cfg;
-
-    if (inv_dmp_open() != INV_SUCCESS) {
-        ALOGE("Fatal Error : could not open DMP correctly.\n");
+    FILE *fptr;
+
+    // get gyro orientation
+    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[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:gyro 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]);
+
+        mGyroOrientation[0] = om[0];
+        mGyroOrientation[1] = om[1];
+        mGyroOrientation[2] = om[2];
+        mGyroOrientation[3] = om[3];
+        mGyroOrientation[4] = om[4];
+        mGyroOrientation[5] = om[5];
+        mGyroOrientation[6] = om[6];
+        mGyroOrientation[7] = om[7];
+        mGyroOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read gyro mounting matrix");
     }
 
-    result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work
-    ALOGE_IF(result != INV_SUCCESS,
-            "Fatal Error : could not set enabled sensors.");
-
-    if (inv_load_calibration() != INV_SUCCESS) {
-        ALOGE("could not open MPL calibration file");
+    // get accel orientation
+    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], 
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:accel 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]);
+
+        mAccelOrientation[0] = om[0];
+        mAccelOrientation[1] = om[1];
+        mAccelOrientation[2] = om[2];
+        mAccelOrientation[3] = om[3];
+        mAccelOrientation[4] = om[4];
+        mAccelOrientation[5] = om[5];
+        mAccelOrientation[6] = om[6];
+        mAccelOrientation[7] = om[7];
+        mAccelOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read accel mounting matrix");
     }
+}
 
-    //check for the 9axis fusion library: if available load it and start 9x
-    void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW);
-    if(h_dmp_lib) {
-        const char* error;
-        error = dlerror();
-        inv_error_t (*fp_inv_enable_9x_fusion)() =
-              (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion");
-        if((error = dlerror()) != NULL) {
-            ALOGE("%s %s", error, "inv_enable_9x_fusion");
-        } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
-            ALOGE( "Warning : 9 axis sensor fusion not available "
-                  "- No compass detected.\n");
-        } else {
-            /*  9axis is loaded and enabled                            */
-            /*  this variable is used for coming up with sensor list   */
-            mNineAxisEnabled = true;
-        }
+MPLSensor::~MPLSensor()
+{
+    VFUNC_LOG;
+
+    /* Close open fds */
+    if (iio_fd > 0)
+        close(iio_fd);
+
+    if( accel_fd > 0 )
+        close(accel_fd );
+    if (gyro_temperature_fd > 0)
+        close(gyro_temperature_fd);
+    if (sysfs_names_ptr)
+        free(sysfs_names_ptr);
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    /* TODO: Turn off and close all sensors */
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp());
+    int fd = open(mpu.chip_enable, O_RDWR);
+    if(fd < 0) {
+        LOGE("HAL:could not open gyro chip enable");
     } else {
-        const char* error = dlerror();
-        ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
+        if(enable_sysfs_sensor(fd, 0) < 0) {
+            LOGE("HAL:could not disable gyro master enable");
+        }
     }
 
-    mldl_cfg = inv_get_dl_config();
+#ifdef INV_PLAYBACK_DBG
+    inv_turn_off_data_logging();
+    fclose(logfile);
+#endif
+}
 
-    if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
-        ALOGE("Error : Bias update function could not be set.\n");
-    }
+#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 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
+
+/* TODO: this step is optional, remove?  */
+int MPLSensor::setGyroInitialState()
+{
+    VFUNC_LOG;
 
-    if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
-        ALOGE("Error : could not set motion interrupt");
-    }
+    int res = 0;
 
-    if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
-        ALOGE("Error : could not set fifo interrupt");
+    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;
+    if(fd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_fifo_rate, strerror(res), res);
+        return res;
     }
-
-    result = inv_set_fifo_rate(6);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
+    res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
+    if(res < 0) {
+        LOGE("HAL:write_attribute_sensor : error writing %s with %d",
+             mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
+        return res;
     }
 
-    mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM;
-    setupCallbacks();
+    // Setting LPF is deprecated
 
+    return 0;
 }
 
-/** setup the fifo contents.
- */
-void MPLSensor::setupFIFO()
+/* this applies to BMA250 only */
+int MPLSensor::setAccelInitialState()
 {
-    FUNC_LOG;
-    inv_error_t result;
-
-    result = inv_send_accel(INV_ALL, INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_accel returned %d\n", result);
-    }
+    VFUNC_LOG;
 
-    result = inv_send_quaternion(INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_quaternion returned %d\n", result);
+    struct input_absinfo absinfo_x;
+    struct input_absinfo absinfo_y;
+    struct input_absinfo absinfo_z;
+    float value;
+    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
+        value = absinfo_x.value;
+        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
+        value = absinfo_y.value;
+        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
+        value = absinfo_z.value;
+        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
+        //mHasPendingEvent = true;
     }
+    return 0;
+}
 
-    result = inv_send_linear_accel(INV_ALL, INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result);
-    }
+int MPLSensor::onPower(int en)
+{
+    VFUNC_LOG;
 
-    result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n",
-             result);
+    int res = 0;
+    char buf[sizeof(int)+1];
+    int count, curr_power_state;
+
+    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;
+    if(tempFd < 0){
+        LOGE("HAL:Open of %s failed with '%s' (%d)",
+             mpu.power_state, strerror(res), res);
+    } else {
+        // check and set new power state
+        count = read_attribute_sensor(tempFd, buf, sizeof(buf));
+        if(count < 1) {
+            LOGE("HAL:Error reading power state");
+            // will set power_state anyway
+            curr_power_state= -1;
+        } else {
+            sscanf(buf, "%d", &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", 
+                    curr_power_state, en);
+            close(tempFd);   
+        }
     }
+    return res;
+}
 
-    result = inv_send_gravity(INV_ALL, INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_gravity returned %d\n", result);
-    }
+int MPLSensor::onDMP(int en)
+{
+    VFUNC_LOG;
 
-    result = inv_send_gyro(INV_ALL, INV_32_BIT);
-    if (result != INV_SUCCESS) {
-        ALOGE("Fatal error: inv_send_gyro returned %d\n", result);
+    int res= -1;
+    int status;
+
+    //Sequence to enable DMP
+    //1. Turn On power if not already on
+    //2. Load DMP image if not already loaded
+    //3. Either Gyro or Accel must be enabled/configured before next step
+    //4. Enable DMP
+
+    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
+        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
+            LOGE("HAL:ERR can't read DMP state");
+        } else if (status != en) {
+            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
+                LOGE("HAL:ERR can't write dmp_on");  
+            } else {
+                res= 0;	//Indicate write successful
+            }
+            //Enable DMP interrupt
+            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
+                LOGE("HAL:ERR can't en/dis DMP interrupt");  
+            }
+        } else {
+            res= 0;  	//DMP already set as requested
+        }
+    } else {
+        LOGE("HAL:ERR No DMP image");
     }
-
+    return res;
 }
 
-/**
- *  set up the callbacks that we use in all cases (outside of gestures, etc)
- */
-void MPLSensor::setupCallbacks()
+int MPLSensor::checkLPQuaternion(void)
 {
-    FUNC_LOG;
-    if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) {
-        ALOGE("Error : Motion callback could not be set.\n");
+    VFUNC_LOG;
 
-    }
+    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
+}
 
-    if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
-        ALOGE("Error : Processed data callback could not be set.");
+int MPLSensor::enableLPQuaternion(int en)
+{
+    VFUNC_LOG;
 
+    if (!en) {
+        enableQuaternionData(0);
+        onDMP(0);
+        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
+    } else {
+        if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {    
+            LOGE("HAL:ERR can't enable LP Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_QUATERNION;
+            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
+        }
     }
+    return 0;
 }
 
-/**
- * handle the motion/no motion output from the MPL.
- */
-void MPLSensor::cbOnMotion(uint16_t val)
+int MPLSensor::enableQuaternionData(int en)
 {
-    FUNC_LOG;
-    //after the first no motion, the gyro should be calibrated well
-    if (val == 2) {
-        mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
-        if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
-            //if gyros are on and we got a no motion, set a flag
-            // indicating that the cal file can be written.
-            mHaveGoodMpuCal = true;
+    int res= 0;
+    VFUNC_LOG;
+
+    //Enable DMP quaternion
+    if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
+        LOGE("HAL:ERR can't write DMP quaternion_on");
+        res= -1;	//Indicate an err
+    } 
+
+    if (!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");
+        if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_r_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_x_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_y_en");
+        }
+        if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
+            LOGE("HAL:ERR write in_quat_z_en"); 
         }
     }
 
-    return;
-}
+    return res;
 
+}
 
-void MPLSensor::cbProcData()
+int MPLSensor::enableTap(int en)
 {
-    mNewData = 1;
-    mSampleCount++;
-    //ALOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount);
-}
+    VFUNC_LOG;
 
-//these handlers transform mpl data into one of the Android sensor types
-//  scaling and coordinate transforms should be done in the handlers
+    return 0;
+}
 
-void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask,
-                             int index)
+int MPLSensor::enableFlick(int en)
 {
     VFUNC_LOG;
-    inv_error_t res;
-    res = inv_get_float_array(INV_GYROS, s->gyro.v);
-    s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0;
-    s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0;
-    s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0;
-    s->gyro.status = mMpuAccuracy;
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
-}
-
-void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask,
-                              int index)
-{
-    //VFUNC_LOG;
-    inv_error_t res;
-    res = inv_get_float_array(INV_ACCELS, s->acceleration.v);
-    //res = inv_get_accel_float(s->acceleration.v);
-    s->acceleration.v[0] = s->acceleration.v[0] * 9.81;
-    s->acceleration.v[1] = s->acceleration.v[1] * 9.81;
-    s->acceleration.v[2] = s->acceleration.v[2] * 9.81;
-    //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
-    s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
-}
-
-int MPLSensor::estimateCompassAccuracy()
-{
-    inv_error_t res;
-    int rv;
 
-    res = inv_get_compass_accuracy(&rv);
-    if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) {
-         mHaveGoodCompassCal = true;	 
-    }
-    ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy");
-
-    return rv;
+    return 0;
 }
 
-void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask,
-                                int index)
+int MPLSensor::enablePedometer(int en)
 {
     VFUNC_LOG;
-    inv_error_t res, res2;
-    float bias_error[3];
-    float total_be;
-    static int bias_error_settled = 0;
 
-    res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
+    return 0;
+}
 
-    if (res != INV_SUCCESS) {
-        ALOGW(
-             "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d",
-             res);
-    }
+int MPLSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
 
-    s->magnetic.status = estimateCompassAccuracy();
+    int res = 0;
 
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
+    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;
+    if(tempFd < 0){
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.chip_enable, strerror(res), res);
+        return res;
+    }
+    res = enable_sysfs_sensor(tempFd, en);
+    return res;
 }
 
-void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask,
-                           int index)
+int MPLSensor::enableGyro(int en)
 {
     VFUNC_LOG;
-    float quat[4];
-    float norm = 0;
-    float ang = 0;
-    inv_error_t r;
 
-    r = inv_get_float_array(INV_QUATERNION, quat);
+    int res = 0;
 
-    if (r != INV_SUCCESS) {
-        *pending_mask &= ~(1 << index);
-        return;
+    /* need to also turn on/off the master enable */
+    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;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, en);
     } else {
-        *pending_mask |= (1 << index);
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_enable, strerror(res), res);
     }
 
-    norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]
-            + FLT_EPSILON;
+    if (!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)", 
+                en, mpu.gyro_x_fifo_enable, getTimestamp());
+        tempFd = open(mpu.gyro_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)",
+                 mpu.gyro_x_fifo_enable, strerror(res), res);
+        }
+
+        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;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.gyro_y_fifo_enable, strerror(res), res);
+        }
 
-    if (norm > 1.0f) {
-        //renormalize
-        norm = sqrtf(norm);
-        float inv_norm = 1.0f / norm;
-        quat[1] = quat[1] * inv_norm;
-        quat[2] = quat[2] * inv_norm;
-        quat[3] = quat[3] * inv_norm;
+        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;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.gyro_z_fifo_enable, strerror(res), res);
+        }
     }
 
-    if (quat[0] < 0.0) {
-        quat[1] = -quat[1];
-        quat[2] = -quat[2];
-        quat[3] = -quat[3];
+    return res;
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    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);
+   }
+
+    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)", 
+                en, mpu.accel_x_fifo_enable, getTimestamp());
+        tempFd = open(mpu.accel_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)",
+                 mpu.accel_x_fifo_enable, strerror(res), res);
+        }
+
+        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;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.accel_y_fifo_enable, strerror(res), res);
+        }
+
+        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;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 mpu.accel_z_fifo_enable, strerror(res), res);
+        }
     }
 
-    s->gyro.v[0] = quat[1];
-    s->gyro.v[1] = quat[2];
-    s->gyro.v[2] = quat[3];
+    if (!en && USE_THIRD_PARTY_ACCEL == 0) {
+    } 
 
-    s->gyro.status
-            = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy
-                                                            : estimateCompassAccuracy());
+    if(USE_THIRD_PARTY_ACCEL == 1 && en) {
+        setAccelInitialState(); // BMA250
+    }
+    return res;
 }
 
-void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
-                           int index)
+int MPLSensor::enableCompass(int en)
 {
     VFUNC_LOG;
-    inv_error_t res;
-    res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v);
-    s->gyro.v[0] *= 9.81;
-    s->gyro.v[1] *= 9.81;
-    s->gyro.v[2] *= 9.81;
-    s->gyro.status = mMpuAccuracy;
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
-}
-
-void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask,
-                             int index)
+
+    int res = mCompassSensor->enable(ID_M, en);
+    if (en == 0) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
+        inv_compass_was_turned_off();
+    } 
+    return res;
+}
+
+void MPLSensor::computeLocalSensorMask(int enabled_sensors)
 {
     VFUNC_LOG;
-    inv_error_t res;
-    res = inv_get_float_array(INV_GRAVITY, s->gyro.v);
-    s->gyro.v[0] *= 9.81;
-    s->gyro.v[1] *= 9.81;
-    s->gyro.v[2] *= 9.81;
-    s->gyro.status = mMpuAccuracy;
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
+
+    do {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
+            mLocalSensorMask = ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
+            /* Invensense compass cal */
+            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "G ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "G DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+
+        if (A_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "A ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "A DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        /* Invensense compass calibration */
+        if (M_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "M ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "M DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+        }
+    } while (0);
 }
 
-void MPLSensor::calcOrientationSensor(float *R, float *values)
-{
-    float tmp;
+int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
+    VFUNC_LOG;
 
-    //Azimuth
-    if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) {
-        values[0] = (float) atan2f(-R[3], R[0]);
-    } else {
-        values[0] = (float) atan2f(R[1], R[4]);
-    }
-    values[0] *= 57.295779513082320876798154814105f;
-    if (values[0] < 0) {
-        values[0] += 360.0f;
-    }
-    //Pitch
-    tmp = R[7];
-    if (tmp > 1.0f)
-        tmp = 1.0f;
-    if (tmp < -1.0f)
-        tmp = -1.0f;
-    values[1] = -asinf(tmp) * 57.295779513082320876798154814105f;
-    if (R[8] < 0) {
-        values[1] = 180.0f - values[1];
-    }
-    if (values[1] > 180.0f) {
-        values[1] -= 360.0f;
-    }
-    //Roll
-    if ((R[7] > 0.7071067f)) {
-        values[2] = (float) atan2f(R[6], R[7]);
-    } else {
-        values[2] = (float) atan2f(R[6], R[8]);
+    inv_error_t res = -1;
+    int on = 1;
+    int off = 0;
+
+    // Sequence to enable or disable a sensor
+    // 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))) {
+        /* ensure power state is on */
+        onPower(1);
+      
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            return res;
+        }
     }
 
-    values[2] *= 57.295779513082320876798154814105f;
-    if (values[2] > 90.0f) {
-        values[2] = 180.0f - values[2];
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
+
+    if (changed & (1 << Gyro)) {
+        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) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
+            res = enableGyro(off);
+            if(res < 0) {
+                return res;
+            }
+        }
     }
-    if (values[2] < -90.0f) {
-        values[2] = -180.0f - values[2];
+
+    if (changed & (1 << Accelerometer)) {
+        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) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
+            res = enableAccel(off);
+            if(res < 0) {
+                return res;
+            }
+        }
+    }
+
+    if (changed & (1 << MagneticField)) {
+        /* Invensense compass calibration */
+        if (sensors & INV_THREE_AXIS_COMPASS) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable compass");
+            res = enableCompass(on);
+            if(res < 0) {
+                return res;
+            }
+        } else if ((sensors & INV_THREE_AXIS_COMPASS) == 0) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable compass");
+            res = enableCompass(off);
+            if(res < 0) {
+                return res;
+            }
+        }
+    }
+
+// 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");
+        }
+    } 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) |
+            (mCompassSensor->isIntegrated() << MagneticField))) {
+        if (sensors & 
+            (INV_THREE_AXIS_GYRO 
+                | INV_THREE_AXIS_ACCEL 
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+            if(res < 0) {
+                return res;
+            }
+        } else { // all sensors idle -> reduce power
+            res = onPower(0);
+            if(res < 0) {
+                return res;
+            }
+            storeCalibration();
+        }
     }
+
+    return res;
 }
 
-void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask,
-                              int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output
+/* Store calibration file */
+void MPLSensor::storeCalibration()
 {
-    VFUNC_LOG;
-    inv_error_t res;
-    float euler[3];
-    float heading[1];
-    float rot_mat[9];
+    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
+       int res = inv_store_calibration();
+       if (res) {
+           LOGE("HAL:Cannot store calibration on file");
+       } else {
+           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
+       }
+    }
+}
 
-    res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
+void MPLSensor::cbProcData()
+{
+    mNewData = 1;
+    mSampleCount++;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
+}
 
-    //ComputeAndOrientation(heading[0], euler, s->orientation.v);
-    calcOrientationSensor(rot_mat, s->orientation.v);
+/*  these handlers transform mpl data into one of the Android sensor types */
+int MPLSensor::gyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
 
-    s->orientation.status = estimateCompassAccuracy();
+int MPLSensor::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    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->timestamp, update);
+    mAccelAccuracy = s->acceleration.status;
+    return update;
+}
 
-    if (res == INV_SUCCESS)
-        *pending_mask |= (1 << index);
-    else
-        ALOGW("orienHandler: data not valid (%d)", (int) res);
+int MPLSensor::compassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_magnetic_field(
+        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);
+    return update;
+}
+
+int MPLSensor::rvHandler(sensors_event_t* s)
+{
+    // rotation vector does not have an accuracy or status
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::laHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &s->gyro.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:la 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::gravHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gr 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::orienHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_orientation(
+            s->orientation.v, &s->orientation.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
+            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
+    return update;
 }
 
 int MPLSensor::enable(int32_t handle, int en)
 {
-    FUNC_LOG;
-    //ALOGV("handle : %d en: %d", handle, en);
+    VFUNC_LOG;
 
+    android::String8 sname;
     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;
     }
 
@@ -845,81 +1353,227 @@ int MPLSensor::enable(int32_t handle, int en)
 
     int newState = en ? 1 : 0;
     int err = 0;
-    //ALOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)),
-    //        "sensor state change what=%d", what);
+    unsigned long sen_mask;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
+            ((mEnabled & (1 << what)) ? "en" : "dis"),
+            ((uint32_t(newState) << what) ? "en" : "dis"));
+    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);
 
-    pthread_mutex_lock(&mMplMutex);
     if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
         uint32_t sensor_type;
         short flags = newState;
+        uint32_t lastEnabled = mEnabled, changed = 0;
+
         mEnabled &= ~(1 << what);
         mEnabled |= (uint32_t(flags) << what);
-        ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled);
-        setPowerStates(mEnabled);
-        pthread_mutex_unlock(&mMplMutex);
-        if (!newState)
-            update_delay();
-        return err;
-    }
-    pthread_mutex_unlock(&mMplMutex);
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
+        computeLocalSensorMask(mEnabled);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
+        sen_mask = mLocalSensorMask & mMasterSensorMask;
+        mSensorMask = sen_mask;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
+
+        switch (what) {
+            case Gyro:
+            case Accelerometer:
+            case MagneticField:
+                if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))) &&
+                        ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                break;
+
+            case Orientation:
+            case RotationVector:
+            case LinearAccel:
+            case Gravity:
+                if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity)))) ||
+                        (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))))) {
+                    for (int i = Gyro; i <= MagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
+        enableSensors(sen_mask, flags, changed);
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
+#ifdef INV_PLAYBACK_DBG
+    /* apparently the logging needs to be go through this sequence 
+       to properly flush the log file */
+    inv_turn_off_data_logging();
+    fclose(logfile);
+    logfile = fopen("/data/playback.bin", "ab");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
     return err;
 }
 
 int MPLSensor::setDelay(int32_t handle, int64_t ns)
 {
-    FUNC_LOG;
-    ALOGV_IF(EXTRA_VERBOSE,
-            " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL));
+    VFUNC_LOG;
+
+    android::String8 sname;
     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:
+    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;
+    if (!(mEnabled & (1 << what))) {
+        counter_delay = 0;
+    } else {
+        if (++counter_delay == 1) {
+            return 0;
+        }
+        else {
+            counter_delay = 0;
+        }
+    }
+#endif
+
     if (uint32_t(what) >= numSensors)
         return -EINVAL;
 
     if (ns < 0)
         return -EINVAL;
 
-    pthread_mutex_lock(&mMplMutex);
+    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) {
+        ns = 10000000LL;
+    }
+*/
+    if (ns < 5000000LL) {
+        ns = 5000000LL;
+    }
+
+    /* store request rate to mDelays arrary for each sensor */
     mDelays[what] = ns;
-    pthread_mutex_unlock(&mMplMutex);
-    return update_delay();
+
+    switch (what) {
+        case Gyro:
+        case Accelerometer:
+            for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
+                    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;
+                }
+            }
+            break;
+
+        case MagneticField:
+            if (mCompassSensor->isIntegrated() &&
+                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
+                 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
+                 return 0;
+            }
+            break;
+
+        case Orientation:
+        case RotationVector:
+        case LinearAccel:
+        case Gravity:
+            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;
+                }
+            }
+            break;
+    }
+
+    // TODO: disabled for GED tablet
+    // pthread_mutex_lock(&mHALMutex);
+    int res = update_delay();
+    // pthread_mutex_unlock(&mHALMutex);
+    return res;
 }
 
-int MPLSensor::update_delay()
-{
-    FUNC_LOG;
-    int rv = 0;
-    bool irq_set[5];
+int MPLSensor::update_delay() {
+    VHANDLER_LOG;
 
-    pthread_mutex_lock(&mMplMutex);
+    int res = 0;
+    int64_t got;
 
     if (mEnabled) {
         uint64_t wanted = -1LLU;
+        uint64_t wanted_ec = -1LLU;
+
+        // 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);
+
+        // TODO: unnecessary for IIO
+        // reset master enable
+        // masterEnable(0);
+
+        /* search the minimum delay requested across all enabled sensors */
         for (int i = 0; i < numSensors; i++) {
             if (mEnabled & (1 << i)) {
                 uint64_t ns = mDelays[i];
@@ -927,201 +1581,687 @@ int MPLSensor::update_delay()
             }
         }
 
-        //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
-        if (wanted < 10000000LLU) {
-            wanted = 10000000LLU;
+	      // same delay for ext compass
+        wanted_ec = wanted;
+
+        /* mpl rate in us in future maybe different for
+           gyro vs compass vs accel */
+        int rateInus = (int)wanted / 1000LL;
+        int mplGyroRate = rateInus;
+        int mplAccelRate = rateInus;
+        int mplCompassRate = rateInus;
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
+             "%llu ns, mpl rate: %d us, (%.2f Hz)",
+             wanted, rateInus, 1000000000.f / wanted);
+
+        /* set rate in MPL */
+        /* compass can only do 100Hz max */
+        inv_set_gyro_sample_rate(mplGyroRate);
+        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);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
+
+        int enabled_sensors = mEnabled;
+        int tempFd = -1;
+        if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            uint64_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, 
+                    getTimestamp());
+            tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
+            if(res < 0) {
+                LOGE("HAL:GYRO update delay error");
+            }
 
-        int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1
-                                                                         : 0); //mpu fifo rate is in increments of 5ms
-        if (rate == 0) //KLP disallow fifo rate 0
-            rate = 1;
+            //nsToHz (BMA250)
+            if(USE_THIRD_PARTY_ACCEL == 1) {
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted / 1000000L, mpu.accel_fifo_rate, 
+                        getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
 
-        if (rate != mCurFifoRate) {
-            //ALOGD("set fifo rate: %d %llu", rate, wanted);
-            inv_error_t res; // = inv_dmp_stop();
-            res = inv_set_fifo_rate(rate);
-            ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
+            if (!mCompassSensor->isIntegrated()) {
+                mCompassSensor->setDelay(ID_M, wanted_ec);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
 
-            //res = inv_dmp_start();
-            //ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
+            /*
+            //nsTons - nothing to be done
+            strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len],
+                   COMPASS_SENSOR_DELAY);
+            tempFd = open(compass_sensor_sysfs_path, O_RDWR);
+            LOGV_IF(PROCESS_VERBOSE, "setDelay - open path: %s", compass_sensor_sysfs_path);
+            wanted = 20000000LLU;
+            res = write_attribute_sensor(tempFd, wanted);
+            if(res < 0) {
+                LOGE("Compass update delay error");
+            }
+            */
 
-            mCurFifoRate = rate;
-            rv = (res == INV_SUCCESS);
-        }
+        } else {
 
-        if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) {
-            if (mUseTimerirq) {
-                ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
-                clearIrqData(irq_set);
-                if (inv_get_dl_config()->requested_sensors
-                        == INV_THREE_AXIS_COMPASS) {
-                    ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
-                          (unsigned long) (wanted / 1000000LLU));
-                    ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
-                            (int) (wanted / 1000000LLU));
+            if (GY_ENABLED) {
+                wanted = mDelays[Gyro];
+                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);
+                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+                LOGE_IF(res < 0, "HAL:GYRO update delay error");
+            }
+
+            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 {
+                    wanted = mDelays[Accelerometer];
+                }
+                /* 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);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            /* Invensense compass calibration */
+            if (M_ENABLED) {
+                if (!mCompassSensor->isIntegrated()) {
+                    wanted = mDelays[MagneticField];
                 } else {
-                    ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
-                          (unsigned long) inv_get_sample_step_size_ms());
-                    ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
-                            (int) inv_get_sample_step_size_ms());
+                    if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[Gyro];
+                    } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = mDelays[MagneticField];
+                    }
                 }
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
             }
+
         }
 
+        /*
+            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 
+                | (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
+        }
     }
-    pthread_mutex_unlock(&mMplMutex);
-    return rv;
-}
-
-/* return the current time in nanoseconds */
-int64_t MPLSensor::now_ns(void)
-{
-    //FUNC_LOG;
-    struct timespec ts;
 
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    //ALOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec);
-    return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+    return res;
 }
 
-int MPLSensor::readEvents(sensors_event_t* data, int count)
+/* use for third party accel */
+int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
 {
-    //VFUNC_LOG;
-    int i;
-    bool irq_set[5] = { false, false, false, false, false };
-    inv_error_t rv;
+    VHANDLER_LOG;
+
     if (count < 1)
         return -EINVAL;
-    int numEventReceived = 0;
-
-    clearIrqData(irq_set);
 
-    pthread_mutex_lock(&mMplMutex);
-    if (mDmpStarted) {
-        //ALOGV_IF(EXTRA_VERBOSE, "Update Data");
-        rv = inv_update_data();
-        ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
+    ssize_t n = mAccelInputReader.fill(accel_fd);
+    if (n < 0) {
+        LOGE("HAL:missed accel events, exit");
+        return n;
     }
 
-    else {
-        //probably just one extra read after shutting down
-        ALOGV_IF(EXTRA_VERBOSE,
-                "MPLSensor::readEvents called, but there's nothing to do.");
+    int numEventReceived = 0;
+    input_event const* event;
+    int nb, done = 0;
+
+    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_ABS) {
+            if (event->code == EVENT_TYPE_ACCEL_X) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[0] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[1] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[2] =event-> value;
+            }
+        } else if (type == EV_SYN) {
+            done = 1;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, getTimestamp());
+            }
+        } else {
+            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
+                    type, event->code);
+        }
+        mAccelInputReader.next();
     }
 
-    pthread_mutex_unlock(&mMplMutex);
+    LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);
 
-    if (!mNewData) {
-        ALOGV_IF(EXTRA_VERBOSE, "no new data");
-        return 0;
+    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.
+ *  @returns 0, if successful, error number if not.
+ */
+int MPLSensor::executeOnData(sensors_event_t* data, int count)
+{
+    VFUNC_LOG;
+
+    inv_execute_on_data();
+
+    int numEventReceived = 0;
+
+    long msg;
+    msg = inv_get_message_level_0(1);
+    if (msg) {
+        if (msg & INV_MSG_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
+        }
+        if (msg & INV_MSG_NO_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
+            /* after the first no motion, the gyro should be
+               calibrated well */
+            mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
+            /* if gyros are on and we got a no motion, set a flag
+               indicating that the cal file can be written. */
+            mHaveGoodMpuCal = true;
+        }
     }
-    mNewData = 0;
-    
-    /* google timestamp */
-    pthread_mutex_lock(&mMplMutex);
+
+    // load up virtual sensors
     for (int i = 0; i < numSensors; i++) {
+        int update;
         if (mEnabled & (1 << i)) {
-            CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i,
-                                              &mPendingMask, i);
-	    mPendingEvents[i].timestamp = irq_timestamp;
-        }
-    }
+            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
+            mPendingMask |= (1 << i);
 
-    for (int j = 0; count && mPendingMask && j < numSensors; j++) {
-        if (mPendingMask & (1 << j)) {
-            mPendingMask &= ~(1 << j);
-            if (mEnabled & (1 << j)) {
-                *data++ = mPendingEvents[j];
+            if (update && (count > 0)) {
+                *data++ = mPendingEvents[i];
                 count--;
                 numEventReceived++;
             }
         }
+    }
+
+    return numEventReceived;
+}
 
+int MPLSensor::readEvents(sensors_event_t *data, int count) {
+
+
+    int lp_quaternion_on, nbyte;
+    int i, nb, mask = 0, numEventReceived = 0,
+        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) +
+            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1: 0);
+    char *rdata = mIIOBuffer;
+
+    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
+    }
+#endif
+
+    // TODO: disabled for GED tablet
+    // pthread_mutex_lock(&mMplMutex);
+    // pthread_mutex_lock(&mHALMutex);
+
+    size_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);
+    }
+/*
+    LOGI("get one sample of IIO data with size: %d", rsize);
+    LOGI("sensors: %d", sensors);
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0)), *((short *) (rdata + 2)),
+        *((short *) (rdata + 4)));
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
+        mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
+*/
+
+    // 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) {
+        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) {
+            mCachedGyroData[i] = *((short *) (rdata + i * 2));
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+            mCachedAccelData[i] = *((short *) (rdata + i * 2 +
+                ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+        }
+        if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
+            mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
+        }
+    }
+
+    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
+        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
+    if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
+            (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
+        mask |= 4;
+    }
+
+    mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
+    if (mCompassSensor->isIntegrated()) {
+        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]);
+        }
+#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);
+#endif
+    }
+
+    if (mask & 1) {
+        mPendingMask |= 1 << Gyro;
+        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[2], mSensorTimestamp);
+        }
+    }
+
+    if (mask & 2) {
+        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[2], mSensorTimestamp);
+        }
+    }
+
+    if ((mask & 4) && mCompassSensor->isIntegrated()) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        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], 
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
     }
 
-    pthread_mutex_unlock(&mMplMutex);
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+    if (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], 
+                    mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
+    }
+#endif
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
     return numEventReceived;
 }
 
-int MPLSensor::getFd() const
+/* use for both MPUxxxx and third party compass */
+int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
 {
-    //ALOGV("MPLSensor::getFd returning %d", data_fd);
-    return data_fd;
+    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);
+    if (done > 0) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        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], 
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    // pthread_mutex_unlock(&mMplMutex);
+    // pthread_mutex_unlock(&mHALMutex);
+
+    return numEventReceived;
 }
 
-int MPLSensor::getAccelFd() const
+int MPLSensor::getFd() const
 {
-    //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd);
-    return accel_fd;
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
+    return iio_fd;
 }
 
-int MPLSensor::getTimerFd() const
+int MPLSensor::getAccelFd() const
 {
-    //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd);
-    return timer_fd;
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
+    return accel_fd;
 }
 
-int MPLSensor::getPowerFd() const
+int MPLSensor::getCompassFd() const
 {
-    int hdl = (int) inv_get_serial_handle();
-    //ALOGV("MPLSensor::getPowerFd returning %d", hdl);
-    return hdl;
+    VFUNC_LOG;
+    int fd = mCompassSensor->getFd();
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
+    return fd;
 }
 
 int MPLSensor::getPollTime()
 {
+    VHANDLER_LOG;
     return mPollTime;
 }
 
 bool MPLSensor::hasPendingEvents() const
 {
-    //if we are using the polling workaround, force the main loop to check for data every time
+    VHANDLER_LOG;
+    // if we are using the polling workaround, force the main
+    // loop to check for data every time
     return (mPollTime != -1);
 }
 
-void MPLSensor::handlePowerEvent()
+/* TODO: support resume suspend when we gain more info about them*/
+void MPLSensor::sleepEvent()
 {
     VFUNC_LOG;
-    mpuirq_data irqd;
+}
 
-    int fd = (int) inv_get_serial_handle();
-    read(fd, &irqd, sizeof(irqd));
+void MPLSensor::wakeEvent()
+{
+    VFUNC_LOG;
+}
+
+int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (long)(fdata[0] * 65536.f);
+    ldata[1] = (long)(fdata[1] * 65536.f);
+    ldata[2] = (long)(fdata[2] * 65536.f);
+    return 0;
+}
+
+int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (fdata[1] * 65536.f);
+    ldata[1] = (fdata[2] * 65536.f);
+    ldata[2] = (fdata[3] * 65536.f);
+    return 0;
+}
 
-    if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) {
-        //going to sleep
-        sleepEvent();
-    } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) {
-        //waking up
-        wakeEvent();
+int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+            return -1;
+    ldata[0] = (long)fdata[0];
+    ldata[1] = (long)fdata[1];
+    ldata[2] = (long)fdata[2];
+    return 0;
+}
+
+int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (short)fdata[0];
+    ldata[1] = (short)fdata[1];
+    ldata[2] = (short)fdata[2];
+    return 0;
+}
+
+int MPLSensor::inv_read_temperature(long long *data)
+{
+    VHANDLER_LOG;
+
+    int count = 0;
+    char raw_buf[40];
+    long raw = 0;
+
+    long long timestamp = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
+                                  sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading gyro temperature");
+        return -1;
+    }
+
+    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
+
+    if(count < 0) {
+        return -1;
     }
 
-    ioctl(fd, MPU_PM_EVENT_HANDLED, 0);
+    LOGV_IF(ENG_VERBOSE, 
+            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
+            raw, timestamp, count);
+    data[0] = raw;
+    data[1] = timestamp;
+
+    return 0;
 }
 
-void MPLSensor::sleepEvent()
+int MPLSensor::inv_read_dmp_state(int fd)
 {
     VFUNC_LOG;
-    pthread_mutex_lock(&mMplMutex);
-    if (mEnabled != 0) {
-        mForceSleep = true;
-        mOldEnabledMask = mEnabled;
-        setPowerStates(0);
+
+    if(fd < 0)
+        return -1;
+
+    int count = 0;
+    char raw_buf[10];
+    short raw = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading dmp state");
+        close(fd);
+        return -1;
+    }
+    count = sscanf(raw_buf, "%hd", &raw);
+    if(count < 0) {
+        LOGE("HAL:dmp state data is invalid");
+        close(fd);
+        return -1;
     }
-    pthread_mutex_unlock(&mMplMutex);
+    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
+    close(fd);
+    return (int)raw;
 }
 
-void MPLSensor::wakeEvent()
+int MPLSensor::inv_read_sensor_bias(int fd, long *data)
 {
     VFUNC_LOG;
-    pthread_mutex_lock(&mMplMutex);
-    if (mForceSleep) {
-        setPowerStates((mOldEnabledMask | mEnabled));
+
+    if(fd == -1) {
+        return -1;
     }
-    mForceSleep = false;
-    pthread_mutex_unlock(&mMplMutex);
+
+    char buf[50];
+    char x[15], y[15], z[15];
+
+    memset(buf, 0, sizeof(buf));
+    int count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        LOGE("HAL:Error reading gyro bias");
+        return -1;
+    }
+    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)", 
+                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)", 
+                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)
@@ -1130,10 +2270,12 @@ void MPLSensor::wakeEvent()
 
 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
 {
+    VFUNC_LOG;
+
     int numsensors;
 
-    if(len < 7*sizeof(sensor_t)) {
-        ALOGE("sensor list too small, not populating.");
+    if(len < (int)(7 * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
         return 0;
     }
 
@@ -1142,226 +2284,272 @@ int MPLSensor::populateSensorList(struct sensor_t *list, int len)
 
     /* first add gyro, accel and compass to the list */
 
-    /* fill in accel values                          */
-    unsigned short accelId = inv_get_accel_id();
-    if(accelId == 0)
-    {
-	ALOGE("Can not get accel id");
-    }   
-    fillAccel(accelId, list);
-
-    /* fill in compass values                        */
-    unsigned short compassId = inv_get_compass_id();
-    if(compassId == 0)
-    {
-	ALOGE("Can not get compass id");
-    }  
-    fillCompass(compassId, list);
-
-    /* fill in gyro values                           */
-    fillGyro(MPU_NAME, list);
-
-    if(mNineAxisEnabled)
-    {
+    /* fill in gyro/accel values */
+    if(chip_ID == NULL) {
+        LOGE("HAL:Can not get gyro/accel id");
+    }
+    fillGyro(chip_ID, list);
+    fillAccel(chip_ID, list);
+
+    // TODO: need fixes for unified HAL and 3rd-party solution
+    mCompassSensor->fillList(&list[MagneticField]);
+
+    if(1) {
         numsensors = 7;
-        /* all sensors will be added to the list     */
-        /* fill in orientation values	             */
+        /* all sensors will be added to the list
+           fill in orientation values */
         fillOrientation(list);
-
-        /* fill in rotation vector values	     */
+        /* fill in rotation vector values */
         fillRV(list);
-
-        /* fill in gravity values			     */
+        /* fill in gravity values */
         fillGravity(list);
-
-        /* fill in Linear accel values            */
+        /* fill in Linear accel values */
         fillLinearAccel(list);
     } else {
         /* no 9-axis sensors, zero fill that part of the list */
         numsensors = 3;
-        memset(list+3, 0, 4*sizeof(struct sensor_t));
+        memset(list + 3, 0, 4 * sizeof(struct sensor_t));
     }
 
     return numsensors;
 }
 
-void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list)
+void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
 {
-    switch (accel) {
-    case ACCEL_ID_LIS331:
-        list[Accelerometer].maxRange = ACCEL_LIS331_RANGE;
-        list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION;
-        list[Accelerometer].power = ACCEL_LIS331_POWER;
-        break;
-
-    case ACCEL_ID_LIS3DH:
-        list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE;
-        list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION;
-        list[Accelerometer].power = ACCEL_LIS3DH_POWER;
-        break;
-
-    case ACCEL_ID_KXSD9:
-        list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE;
-        list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION;
-        list[Accelerometer].power = ACCEL_KXSD9_POWER;
-        break;
-
-    case ACCEL_ID_KXTF9:
-        list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE;
-        list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION;
-        list[Accelerometer].power = ACCEL_KXTF9_POWER;
-        break;
-
-    case ACCEL_ID_BMA150:
-        list[Accelerometer].maxRange = ACCEL_BMA150_RANGE;
-        list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION;
-        list[Accelerometer].power = ACCEL_BMA150_POWER;
-        break;
-
-    case ACCEL_ID_BMA222:
-        list[Accelerometer].maxRange = ACCEL_BMA222_RANGE;
-        list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION;
-        list[Accelerometer].power = ACCEL_BMA222_POWER;
-        break;
-
-    case ACCEL_ID_BMA250:
-        list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
-        list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
-        list[Accelerometer].power = ACCEL_BMA250_POWER;
-        break;
-
-    case ACCEL_ID_ADXL34X:
-        list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE;
-        list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION;
-        list[Accelerometer].power = ACCEL_ADXL34X_POWER;
-        break;
-
-    case ACCEL_ID_MMA8450:
-        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
-        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
-        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
-        break;
-
-    case ACCEL_ID_MMA845X:
-        list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE;
-        list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION;
-        list[Accelerometer].power = ACCEL_MMA845X_POWER;
-        break;
+    VFUNC_LOG;
 
-    case ACCEL_ID_MPU6050:
-        list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
-        list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
-        list[Accelerometer].power = ACCEL_MPU6050_POWER;
-        break;
-    default:
-        ALOGE("unknown accel id -- accel params will be wrong.");
-        break;
+    if (accel) {
+        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
+            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;
+
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9150_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
+            return;
+        } 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;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        }
     }
-}
 
-void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list)
-{
-    switch (compass) {
-    case COMPASS_ID_AK8975:
-        list[MagneticField].maxRange = COMPASS_AKM8975_RANGE;
-        list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION;
-        list[MagneticField].power = COMPASS_AKM8975_POWER;
-        break;
-    case COMPASS_ID_AMI30X:
-        list[MagneticField].maxRange = COMPASS_AMI30X_RANGE;
-        list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION;
-        list[MagneticField].power = COMPASS_AMI30X_POWER;
-        break;
-    case COMPASS_ID_AMI306:
-        list[MagneticField].maxRange = COMPASS_AMI306_RANGE;
-        list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
-        list[MagneticField].power = COMPASS_AMI306_POWER;
-        break;
-    case COMPASS_ID_YAS529:
-        list[MagneticField].maxRange = COMPASS_YAS529_RANGE;
-        list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
-        list[MagneticField].power = COMPASS_AMI306_POWER;
-        break;
-    case COMPASS_ID_YAS530:
-        list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
-        list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
-        list[MagneticField].power = COMPASS_YAS530_POWER;
-        break;
-    case COMPASS_ID_HMC5883:
-        list[MagneticField].maxRange = COMPASS_HMC5883_RANGE;
-        list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION;
-        list[MagneticField].power = COMPASS_HMC5883_POWER;
-        break;
-    case COMPASS_ID_MMC314X:
-        list[MagneticField].maxRange = COMPASS_MMC314X_RANGE;
-        list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION;
-        list[MagneticField].power = COMPASS_MMC314X_POWER;
-        break;
-    case COMPASS_ID_HSCDTD002B:
-        list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE;
-        list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION;
-        list[MagneticField].power = COMPASS_HSCDTD002B_POWER;
-        break;
-    case COMPASS_ID_HSCDTD004A:
-        list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE;
-        list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION;
-        list[MagneticField].power = COMPASS_HSCDTD004A_POWER;
-        break;
-    default:
-        ALOGE("unknown compass id -- compass parameters will be wrong");
-    }
+    LOGE("HAL:unknown accel id %s -- "
+         "params default to bma250 and might be wrong.",
+         accel);
+    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+    list[Accelerometer].power = ACCEL_BMA250_POWER;
+    list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
 }
 
 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
 {
-    if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) {
+    VFUNC_LOG;
+
+    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
         list[Gyro].power = GYRO_MPU3050_POWER;
-    } else {
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
         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;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
+        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9150_POWER;
+        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
+    } else {
+        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
+        LOGE("HAL:default to use mpu3050 params");
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
     }
     return;
 }
 
-
-/* fillRV depends on values of accel and compass in the list	*/
+/* fillRV depends on values of accel and compass in the list */
 void MPLSensor::fillRV(struct sensor_t *list)
 {
+    VFUNC_LOG;
+
     /* compute power on the fly */
-    list[RotationVector].power = list[Gyro].power + list[Accelerometer].power
-            + list[MagneticField].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;
 }
 
 void MPLSensor::fillOrientation(struct sensor_t *list)
 {
-    list[Orientation].power = list[Gyro].power + list[Accelerometer].power
-            + list[MagneticField].power;
+    VFUNC_LOG;
+
+    list[Orientation].power = list[Gyro].power +
+                              list[Accelerometer].power +
+                              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;
 }
 
 void MPLSensor::fillGravity( struct sensor_t *list)
 {
-    list[Gravity].power = list[Gyro].power + list[Accelerometer].power
-            + list[MagneticField].power;
+    VFUNC_LOG;
+
+    list[Gravity].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          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;
 }
 
 void MPLSensor::fillLinearAccel(struct sensor_t *list)
 {
-    list[Gravity].power = list[Gyro].power + list[Accelerometer].power
-            + list[MagneticField].power;
-    list[Gravity].resolution = list[Accelerometer].resolution;
-    list[Gravity].maxRange = list[Accelerometer].maxRange;
+    VFUNC_LOG;
+
+    list[LinearAccel].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          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;
 }
 
+int MPLSensor::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;
+
+    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 {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        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");
+    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
+    sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
+
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
+    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_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+
+    // TODO: for self test
+    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_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
+    sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
+    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");
+
+    // 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.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++);
+    }
+#endif
+    return 0;
+}
diff --git a/libsensors/MPLSensor.h b/libsensors/MPLSensor.h
index 2b1571d..4c38c57 100644
--- a/libsensors/MPLSensor.h
+++ b/libsensors/MPLSensor.h
@@ -1,143 +1,282 @@
-/*
- * Copyright (C) 2011 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.
- */
-/*************Removed the gesture related info for Google check in : Meenakshi Ramamoorthi: May 31st *********/
-
-#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"
-
-/*****************************************************************************/
-/** 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 void (MPLSensor::*hfunc_t)(sensors_event_t*, uint32_t*, int);
-
-public:
-    MPLSensor();
-    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);
-    virtual int readEvents(sensors_event_t *data, int count);
-    virtual int getFd() const;
-    virtual int getAccelFd() const;
-    virtual int getTimerFd() const;
-    virtual int getPowerFd() const;
-    virtual int getPollTime();
-    virtual bool hasPendingEvents() const;
-    virtual void handlePowerEvent();
-    virtual void sleepEvent();
-    virtual void wakeEvent();
-    int populateSensorList(struct sensor_t *list, int len);
-    void cbOnMotion(uint16_t);
-    void cbProcData();
-
-protected:
-
-    void clearIrqData(bool* irq_set);
-    void setPowerStates(int enabledsensor);
-    void initMPL();
-    void setupFIFO();
-    void setupCallbacks();
-    void gyroHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void accelHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void compassHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void rvHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void laHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void gravHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void orienHandler(sensors_event_t *data, uint32_t *pendmask, int index);
-    void calcOrientationSensor(float *Rx, float *Val);
-    int estimateCompassAccuracy();
-
-    int mMpuAccuracy; //global storage for the current accuracy status
-    int mNewData; //flag indicating that the MPL calculated new output values
-    int mDmpStarted;
-    long mMasterSensorMask;
-    long mLocalSensorMask;
-    int mPollTime;
-    int mCurFifoRate; //current fifo rate
-    bool mHaveGoodMpuCal; //flag indicating that the cal file can be written
-    bool mHaveGoodCompassCal;
-    bool mUseTimerIrqAccel;
-    bool mUsetimerIrqCompass;
-    bool mUseTimerirq;
-    struct pollfd mPollFds[4];
-    int mSampleCount;
-    pthread_mutex_t mMplMutex;
-    int64_t now_ns();
-    int64_t select_ns(unsigned long long time_set[]);    
-
-    enum FILEHANDLES
-    {
-        MPUIRQ_FD, ACCELIRQ_FD, COMPASSIRQ_FD, TIMERIRQ_FD,
-    };
-
-private:
-
-    int update_delay();
-    int accel_fd;
-    int timer_fd;
-
-    uint32_t mEnabled;
-    uint32_t mPendingMask;
-    sensors_event_t mPendingEvents[numSensors];
-    uint64_t mDelays[numSensors];
-    hfunc_t mHandlers[numSensors];
-    bool mForceSleep;
-    long int mOldEnabledMask;
-    android::KeyedVector<int, int> mIrqFds;
-
-    /* added for dynamic get sensor list              */
-    bool mNineAxisEnabled;
-    void fillAccel(unsigned char accel, struct sensor_t *list);
-    void fillCompass(unsigned char compass, 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 setCallbackObject(MPLSensor*);
-
-/*****************************************************************************/
-
-#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"

+

+// 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();

+

+    //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:

+    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

diff --git a/libsensors/MPLSupport.cpp b/libsensors/MPLSupport.cpp
new file mode 100644
index 0000000..a961d78
--- /dev/null
+++ b/libsensors/MPLSupport.cpp
@@ -0,0 +1,144 @@
+#include <MPLSupport.h>
+#include <string.h>
+#include <stdio.h>
+#include "log.h"
+#include "SensorBase.h"
+#include <fcntl.h>
+
+int inv_read_data(char *fname, long *data)
+{
+    VFUNC_LOG;
+
+    char buf[sizeof(long) * 4];
+    int count, fd;
+
+    fd = open(fname, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening %s", fname);
+        return -1;
+    }
+    memset(buf, 0, sizeof(buf));
+    count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        close(fd);
+        return -1;
+    } else {
+        count = sscanf(buf, "%ld", data);
+        if(count)
+            LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
+    }
+    close(fd);
+
+    return 0;
+}
+
+/* This one DOES NOT close FDs for you */
+int read_attribute_sensor(int fd, char* data, unsigned int size)
+{
+    VFUNC_LOG;
+
+    int count = 0;
+    if (fd > 0) {
+        count = pread(fd, data, size, 0);
+        if(count < 1) {
+            LOGE("HAL:read fails with error code=%d", count);
+        }
+    }
+    return count;
+}
+
+/**
+ *  @brief  Enable a sensor through the sysfs file descriptor
+ *          provided.
+ *  @note   this function one closes FD after the write
+ *  @param  fd
+ *              the file descriptor to write into
+ *  @param  en
+ *              the value to write, typically 1 or 0
+ *  @return the errno whenever applicable.
+ */
+int enable_sysfs_sensor(int fd, int en)
+{
+    VFUNC_LOG;
+
+    int nb = -1;
+    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';
+
+        if (nb <= 0) {
+            err = errno;
+            LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
+                 buf[0], nb, strerror(err), err);
+        }
+        close(fd);
+    } else {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:enable_sysfs_sensor - fd<0");
+    }
+
+    return err;
+}
+
+/* This one closes FDs for you */
+int write_attribute_sensor(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);
+        }
+        close(fd);
+    }
+
+    return num_b;
+}
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "r");
+    if (sysfsfp!=NULL) {
+        fscanf(sysfsfp, "%d\n", var);
+	fclose(sysfsfp);
+    } else {
+        LOGE("HAL:ERR open file to read");
+        res= -1;   
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp!=NULL) {
+        fprintf(sysfsfp, "%d\n", var);
+	fclose(sysfsfp);
+    } else {
+        LOGE("HAL:ERR open file to write");
+        res= -1;   
+    }
+    return res;
+}
diff --git a/libsensors/MPLSupport.h b/libsensors/MPLSupport.h
new file mode 100644
index 0000000..73604ba
--- /dev/null
+++ b/libsensors/MPLSupport.h
@@ -0,0 +1,29 @@
+/*
+* 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_SUPPORT_H
+#define ANDROID_MPL_SUPPORT_H
+
+#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);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/libsensors/SensorBase.cpp b/libsensors/SensorBase.cpp
index 79b1ee2..fd0e2ca 100644
--- a/libsensors/SensorBase.cpp
+++ b/libsensors/SensorBase.cpp
@@ -1,128 +1,143 @@
-/*
- * Copyright (C) 2008 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.
- */
-
-#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);
-        ALOGE_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);
-        if (fd>=0) {
-            char name[80];
-            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
-                name[0] = '\0';
-            }
-            if (!strcmp(name, inputName)) {
-                strcpy(input_name, filename);
-                break;
-            } else {
-                close(fd);
-                fd = -1;
-            }
-        }
-    }
-    closedir(dir);
-    ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName);
-    return fd;
-}
+/*

+* 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;

+}

diff --git a/libsensors/SensorBase.h b/libsensors/SensorBase.h
index bb4d055..d9abe92 100644
--- a/libsensors/SensorBase.h
+++ b/libsensors/SensorBase.h
@@ -1,18 +1,18 @@
 /*
- * Copyright (C) 2008 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.
- */
+* 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_SENSOR_BASE_H
 #define ANDROID_SENSOR_BASE_H
@@ -22,6 +22,9 @@
 #include <sys/cdefs.h>
 #include <sys/types.h>
 
+#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
+
+#define MAX_SYSFS_NAME_LEN  (100)
 
 /*****************************************************************************/
 
@@ -29,27 +32,23 @@ struct sensors_event_t;
 
 class SensorBase {
 protected:
-    const char* dev_name;
-    const char* data_name;
-    char        input_name[PATH_MAX];
-    int         dev_fd;
-    int         data_fd;
+    const char *dev_name;
+    const char *data_name;
+    char input_name[PATH_MAX];
+    int dev_fd;
+    int data_fd;
 
     int openInput(const char* inputName);
     static int64_t getTimestamp();
-
-
     static int64_t timevalToNano(timeval const& t) {
-        return t.tv_sec*1000000000LL + t.tv_usec*1000;
+        return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
     }
 
     int open_device();
     int close_device();
 
 public:
-            SensorBase(
-                    const char* dev_name,
-                    const char* data_name);
+            SensorBase(const char* dev_name, const char* data_name);
 
     virtual ~SensorBase();
 
@@ -57,7 +56,7 @@ public:
     virtual bool hasPendingEvents() const;
     virtual int getFd() const;
     virtual int setDelay(int32_t handle, int64_t ns);
-    virtual int enable(int32_t handle, int enabled) = 0;
+    virtual int enable(int32_t handle, int enabled);
 };
 
 /*****************************************************************************/
diff --git a/libsensors/libmllite.so b/libsensors/libmllite.so
new file mode 100644
index 0000000..ed13b13
--- /dev/null
+++ b/libsensors/libmllite.so
diff --git a/libsensors/libmplmpu.so b/libsensors/libmplmpu.so
new file mode 100644
index 0000000..e2ab461
--- /dev/null
+++ b/libsensors/libmplmpu.so
diff --git a/libsensors/local_log_def.h b/libsensors/local_log_def.h
new file mode 100644
index 0000000..b08fdf1
--- /dev/null
+++ b/libsensors/local_log_def.h
@@ -0,0 +1,46 @@
+#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/sensor_params.h b/libsensors/sensor_params.h
index 4925ac4..88d5ba0 100644
--- a/libsensors/sensor_params.h
+++ b/libsensors/sensor_params.h
@@ -1,134 +1,162 @@
-/*
- * Copyright (C) 2011 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)
-/******************************************/
-//COMPASS_ID_AKM
-#define COMPASS_AKM8975_RANGE       (9830.0f)
-#define COMPASS_AKM8975_RESOLUTION  (0.285f)
-#define COMPASS_AKM8975_POWER       (10.0f)
-//COMPASS_ID_AMI30X
-#define COMPASS_AMI30X_RANGE        (5461.0f)
-#define COMPASS_AMI30X_RESOLUTION   (0.9f)
-#define COMPASS_AMI30X_POWER        (0.15f)
-//COMPASS_ID_AMI306
-#define COMPASS_AMI306_RANGE        (5461.0f)
-#define COMPASS_AMI306_RESOLUTION   (0.9f)
-#define COMPASS_AMI306_POWER        (0.15f)
-//COMPASS_ID_YAS529
-#define COMPASS_YAS529_RANGE        (19660.0f)
-#define COMPASS_YAS529_RESOLUTION   (0.012f)
-#define COMPASS_YAS529_POWER        (4.0f)
-//COMPASS_ID_YAS530
-#define COMPASS_YAS530_RANGE        (8001.0f)
-#define COMPASS_YAS530_RESOLUTION   (0.012f)
-#define COMPASS_YAS530_POWER        (4.0f)
-//COMPASS_ID_HMC5883
-#define COMPASS_HMC5883_RANGE       (10673.0f)
-#define COMPASS_HMC5883_RESOLUTION  (10.0f)
-#define COMPASS_HMC5883_POWER       (0.24f)
-//COMPASS_ID_LSM303DLH
-#define COMPASS_LSM303DLH_RANGE     (10240.0f)
-#define COMPASS_LSM303DLH_RESOLUTION    (1.0f)
-#define COMPASS_LSM303DLH_POWER     (1.0f)
-//COMPASS_ID_LSM303DLM
-#define COMPASS_LSM303DLM_RANGE     (10240.0f)
-#define COMPASS_LSM303DLM_RESOLUTION    (1.0f)
-#define COMPASS_LSM303DLM_POWER     (1.0f)
-//COMPASS_ID_MMC314X
-#define COMPASS_MMC314X_RANGE       (400.0f)
-#define COMPASS_MMC314X_RESOLUTION  (2.0f)
-#define COMPASS_MMC314X_POWER       (0.55f)
-//COMPASS_ID_HSCDTD002B
-#define COMPASS_HSCDTD002B_RANGE    (9830.0f)
-#define COMPASS_HSCDTD002B_RESOLUTION   (1.0f)
-#define COMPASS_HSCDTD002B_POWER    (1.0f)
-//COMPASS_ID_HSCDTD004A
-#define COMPASS_HSCDTD004A_RANGE    (9830.0f)
-#define COMPASS_HSCDTD004A_RESOLUTION   (1.0f)
-#define COMPASS_HSCDTD004A_POWER    (1.0f)
-/*******************************************/
-//ACCEL_ID_LIS331
-#define ACCEL_LIS331_RANGE      (2.480f*GRAVITY_EARTH)
-#define ACCEL_LIS331_RESOLUTION     (.001f*GRAVITY_EARTH)
-#define ACCEL_LIS331_POWER      (1.0f)
-//ACCEL_ID_LSM303DLX
-#define ACCEL_LSM303DLX_RANGE       (2.480f*GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_RESOLUTION  (0.001f*GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_POWER       (1.0f)
-//ACCEL_ID_LIS3DH
-#define ACCEL_LIS3DH_RANGE      (2.480f*GRAVITY_EARTH)
-#define ACCEL_LIS3DH_RESOLUTION     (0.001f*GRAVITY_EARTH)
-#define ACCEL_LIS3DH_POWER      (1.0f)
-//ACCEL_ID_KXSD9
-#define ACCEL_KXSD9_RANGE       (2.5006f*GRAVITY_EARTH)
-#define ACCEL_KXSD9_RESOLUTION      (0.001f*GRAVITY_EARTH)
-#define ACCEL_KXSD9_POWER       (1.0f)
-//ACCEL_ID_KXTF9
-#define ACCEL_KXTF9_RANGE       (1.0f*GRAVITY_EARTH)
-#define ACCEL_KXTF9_RESOLUTION      (0.033f*GRAVITY_EARTH)
-#define ACCEL_KXTF9_POWER       (0.35f)
-//ACCEL_ID_BMA150
-#define ACCEL_BMA150_RANGE      (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA150_RESOLUTION     (0.004f*GRAVITY_EARTH)
-#define ACCEL_BMA150_POWER      (0.2f)
-//ACCEL_ID_BMA222
-#define ACCEL_BMA222_RANGE      (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA222_RESOLUTION     (0.001f*GRAVITY_EARTH)
-#define ACCEL_BMA222_POWER      (0.1f)
-//ACCEL_ID_BMA250
-#define ACCEL_BMA250_RANGE      (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA250_RESOLUTION     (0.00391f*GRAVITY_EARTH)
-#define ACCEL_BMA250_POWER      (0.139f)
-//ACCEL_ID_ADXL34X
-#define ACCEL_ADXL34X_RANGE     (2.0f*GRAVITY_EARTH)
-#define ACCEL_ADXL34X_RESOLUTION    (0.001f*GRAVITY_EARTH)
-#define ACCEL_ADXL34X_POWER     (1.0f)
-//ACCEL_ID_MMA8450
-#define ACCEL_MMA8450_RANGE     (2.0f*GRAVITY_EARTH)
-#define ACCEL_MMA8450_RESOLUTION    (0.001f*GRAVITY_EARTH)
-#define ACCEL_MMA8450_POWER     (1.0f)
-//ACCEL_ID_MMA845X
-#define ACCEL_MMA845X_RANGE     (2.0f*GRAVITY_EARTH)
-#define ACCEL_MMA845X_RESOLUTION    (0.001f*GRAVITY_EARTH)
-#define ACCEL_MMA845X_POWER     (1.0f)
-//ACCEL_ID_MPU6050
-#define ACCEL_MPU6050_RANGE     (2.0f*GRAVITY_EARTH)
-#define ACCEL_MPU6050_RESOLUTION    (0.004f*GRAVITY_EARTH)
-#define ACCEL_MPU6050_POWER     (0.0f)
-/******************************************/
-//GYRO MPU3050
-#define RAD_P_DEG (3.14159f/180.0f)
-#define GYRO_MPU3050_RANGE      (2000.0f*RAD_P_DEG)
-#define GYRO_MPU3050_RESOLUTION     (32.8f*RAD_P_DEG)
-#define GYRO_MPU3050_POWER      (6.1f)
-//GYRO MPU6050
-#define GYRO_MPU6050_RANGE      (2000.0f*RAD_P_DEG)
-#define GYRO_MPU6050_RESOLUTION     (16.4f*RAD_P_DEG)
-#define GYRO_MPU6050_POWER      (5.5f)
-
-#endif
-
+/*

+* 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 */

+

diff --git a/libsensors/sensors.h b/libsensors/sensors.h
index 5c56da0..0556c10 100644
--- a/libsensors/sensors.h
+++ b/libsensors/sensors.h
@@ -1,53 +1,104 @@
-/*
- * Copyright (C) 2008 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 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
-
-/*****************************************************************************/
-
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-
-#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)
-
-/*****************************************************************************/
-
-/*
- * The SENSORS Module
- */
-
-__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

+

+#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

diff --git a/libsensors/sensors_mpl.cpp b/libsensors/sensors_mpl.cpp
new file mode 100644
index 0000000..990c5f5
--- /dev/null
+++ b/libsensors/sensors_mpl.cpp
@@ -0,0 +1,236 @@
+/*

+* 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;

+}

diff --git a/libsensors/software/build/android/common.mk b/libsensors/software/build/android/common.mk
new file mode 100644
index 0000000..b84a99c
--- /dev/null
+++ b/libsensors/software/build/android/common.mk
@@ -0,0 +1,68 @@
+# Use bash for additional echo fancyness
+SHELL = /bin/bash
+
+####################################################################################################
+## defines
+
+## libraries ##
+LIB_PREFIX = lib
+
+STATIC_LIB_EXT = a
+SHARED_LIB_EXT = so
+
+# normally, overridden from outside 
+# ?= assignment sets it only if not already defined
+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
+
+ANDROID_LINK  = -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib 
+
+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)/external/skia/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates
+
+KERNEL_INCLUDES  = -I$(KERNEL_ROOT)/include
+
+INV_INCLUDES  = -I$(INV_ROOT)/software/core/driver/include
+INV_INCLUDES += -I$(MLLITE_DIR)
+INV_INCLUDES += -I$(MLLITE_DIR)/linux
+
+INV_DEFINES += -DINV_CACHE_DMP=1
+
+####################################################################################################
+## macros
+
+ifndef echo_in_colors
+define echo_in_colors
+	echo -ne "\e[1;32m"$(1)"\e[0m"
+endef 
+endif
+
+
+
diff --git a/libsensors/software/build/android/shared.mk b/libsensors/software/build/android/shared.mk
new file mode 100644
index 0000000..bc8548c
--- /dev/null
+++ b/libsensors/software/build/android/shared.mk
@@ -0,0 +1,74 @@
+SHELL=/bin/bash
+
+TARGET       ?= android
+PRODUCT      ?= beagleboard
+ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair
+KERNEL_ROOT  ?= /Android/trunk/0xdroid/kernel
+MLSDK_ROOT   ?= $(CURDIR)
+
+ifeq ($(VERBOSE),1)
+	DUMP=1>/dev/stdout
+else
+	DUMP=1>/dev/null
+endif
+
+include common.mk
+
+################################################################################
+## targets
+
+INV_ROOT = ../..
+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/self_test/build/$(TARGET)
+
+INSTALL_DIR = $(CURDIR)
+
+################################################################################
+## macros
+
+define echo_in_colors
+	echo -ne "\e[1;34m"$(1)"\e[0m"
+endef
+
+################################################################################
+## rules
+
+.PHONY : all mllite mpl clean
+
+all:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+clean: 
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+cleanall:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+install:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
diff --git a/libsensors/software/core/HAL/include/inv_sysfs_utils.h b/libsensors/software/core/HAL/include/inv_sysfs_utils.h
new file mode 100644
index 0000000..fce28ae
--- /dev/null
+++ b/libsensors/software/core/HAL/include/inv_sysfs_utils.h
@@ -0,0 +1,83 @@
+/**
+ *  @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/software/core/HAL/include/mlos.h b/libsensors/software/core/HAL/include/mlos.h
new file mode 100644
index 0000000..ce06b07
--- /dev/null
+++ b/libsensors/software/core/HAL/include/mlos.h
@@ -0,0 +1,104 @@
+/*
+ $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/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors/software/core/HAL/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..c45badd
--- /dev/null
+++ b/libsensors/software/core/HAL/linux/inv_sysfs_utils.c
@@ -0,0 +1,317 @@
+/**
+ *  @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/mlsdk/platform/linux/mlos_linux.c b/libsensors/software/core/HAL/linux/mlos_linux.c
index fd3b002..75f062e 100644
--- a/mlsdk/platform/linux/mlos_linux.c
+++ b/libsensors/software/core/HAL/linux/mlos_linux.c
@@ -1,19 +1,7 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 /*******************************************************************************
  *
diff --git a/mlsdk/platform/include/linux/mpu.h b/libsensors/software/core/driver/include/linux/mpu.h
index 04fa7b6..9b29695 100644
--- a/mlsdk/platform/include/linux/mpu.h
+++ b/libsensors/software/core/driver/include/linux/mpu.h
@@ -1,19 +1,24 @@
 /*
- $License:
-   Copyright 2011 InvenSense, Inc.
+* Copyright (C) 2012 Invensense, Inc.
+*
+* This software is licensed under the terms of the GNU General Public
+* License version 2, as published by the Free Software Foundation, and
+* may be copied, distributed, and modified under those terms.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+*/
 
- 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.
-  $
+/**
+ *  @addtogroup  DRIVERS
+ *  @brief       Hardware drivers.
+ *
+ *  @{
+ *      @file    mpu.h
+ *      @brief   mpu definition
  */
 
 #ifndef __MPU_H_
@@ -24,22 +29,21 @@
 #include <linux/ioctl.h>
 #elif defined LINUX
 #include <sys/ioctl.h>
+#include <linux/types.h>
+#else
+#include "mltypes.h"
 #endif
 
-/* Number of axes on each sensor */
-#define GYRO_NUM_AXES               (3)
-#define ACCEL_NUM_AXES              (3)
-#define COMPASS_NUM_AXES            (3)
-
 struct mpu_read_write {
 	/* Memory address or register address depending on ioctl */
-	unsigned short address;
-	unsigned short length;
-	unsigned char *data;
+	__u16 address;
+	__u16 length;
+	__u8 *data;
 };
 
 enum mpuirq_data_type {
-	MPUIRQ_DATA_TYPE_MPU_IRQ,
+	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,
@@ -49,38 +53,65 @@ enum mpuirq_data_type {
 #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 {
-	int interruptcount;
-	unsigned long long irqtime;
-	int data_type;
-	long 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_SUSPEND,
-	MPU_SLAVE_CONFIG_IRQ_RESUME,
+	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,
-	/* AKM specific config keys */
-	MPU_SLAVE_READ_SCALE,
-	/* YAS specific config keys */
-	MPU_SLAVE_OFFSET_VALS,
-	MPU_SLAVE_RANGE_CHECK,
-
-	MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS,
+	/* 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 */
@@ -91,9 +122,11 @@ enum ext_slave_config_irq_type {
 };
 
 /* 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
@@ -108,27 +141,40 @@ enum ext_slave_config_irq_type {
  * @data pointer to the data to confgure or get
  */
 struct ext_slave_config {
-	int key;
-	int len;
-	int apply;
+	__u8 key;
+	__u16 len;
+	__u8 apply;
 	void *data;
 };
 
 enum ext_slave_type {
 	EXT_SLAVE_TYPE_GYROSCOPE,
-	EXT_SLAVE_TYPE_ACCELEROMETER,
+	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,
+	SECONDARY_SLAVE_TYPE_COMPASS,
+	SECONDARY_SLAVE_TYPE_PRESSURE,
+
+	SECONDARY_SLAVE_TYPE_TYPES
+};
 
 enum ext_slave_id {
 	ID_INVALID = 0,
+	GYRO_ID_MPU3050,
+	GYRO_ID_MPU6050A2,
+	GYRO_ID_MPU6050B1,
+	GYRO_ID_MPU6050B1_NO_ACCEL,
+	GYRO_ID_ITG3500,
 
 	ACCEL_ID_LIS331,
-	ACCEL_ID_LSM303A,
+	ACCEL_ID_LSM303DLX,
 	ACCEL_ID_LIS3DH,
 	ACCEL_ID_KXSD9,
 	ACCEL_ID_KXTF9,
@@ -140,13 +186,16 @@ enum ext_slave_id {
 	ACCEL_ID_MMA845X,
 	ACCEL_ID_MPU6050,
 
+	COMPASS_ID_AK8963,
 	COMPASS_ID_AK8975,
+	COMPASS_ID_AK8972,
 	COMPASS_ID_AMI30X,
 	COMPASS_ID_AMI306,
 	COMPASS_ID_YAS529,
 	COMPASS_ID_YAS530,
 	COMPASS_ID_HMC5883,
-	COMPASS_ID_LSM303M,
+	COMPASS_ID_LSM303DLH,
+	COMPASS_ID_LSM303DLM,
 	COMPASS_ID_MMC314X,
 	COMPASS_ID_HSCDTD002B,
 	COMPASS_ID_HSCDTD004A,
@@ -154,6 +203,8 @@ enum ext_slave_id {
 	PRESSURE_ID_BMA085,
 };
 
+#define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
+
 enum ext_slave_endian {
 	EXT_SLAVE_BIG_ENDIAN,
 	EXT_SLAVE_LITTLE_ENDIAN,
@@ -172,8 +223,8 @@ enum ext_slave_bus {
  *  struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
  *  slave devices
  *
- *  @get_slave_descr: Function pointer to retrieve the struct ext_slave_descr
- *                    for this slave
+ *  @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
@@ -189,19 +240,19 @@ enum ext_slave_bus {
  * column should have exactly 1 non-zero value.
  */
 struct ext_slave_platform_data {
-	struct ext_slave_descr *(*get_slave_descr) (void);
-	int irq;
-	int adapt_num;
-	int bus;
-	unsigned char address;
-	signed char orientation[9];
+	__u8 type;
+	__u32 irq;
+	__u32 adapt_num;
+	__u32 bus;
+	__u8 address;
+	__s8 orientation[9];
 	void *irq_data;
 	void *private_data;
 };
 
 struct fix_pnt_range {
-	long mantissa;
-	long fraction;
+	__s32 mantissa;
+	__s32 fraction;
 };
 
 static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
@@ -210,8 +261,8 @@ static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
 }
 
 struct ext_slave_read_trigger {
-	unsigned char reg;
-	unsigned char value;
+	__u8 reg;
+	__u8 value;
 };
 
 /**
@@ -228,8 +279,8 @@ struct ext_slave_read_trigger {
  *  @name:	text name of the device
  *  @type:	device type. enum ext_slave_type
  *  @id:	enum ext_slave_id
- *  @reg:	starting register address to retrieve data.
- *  @len:	length in bytes of the sensor data.  Should be 6.
+ *  @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
@@ -254,7 +305,7 @@ struct ext_slave_descr {
 	int (*read) (void *mlsl_handle,
 		     struct ext_slave_descr *slave,
 		     struct ext_slave_platform_data *pdata,
-		     unsigned char *data);
+		     __u8 *data);
 	int (*config) (void *mlsl_handle,
 		       struct ext_slave_descr *slave,
 		       struct ext_slave_platform_data *pdata,
@@ -265,11 +316,11 @@ struct ext_slave_descr {
 			   struct ext_slave_config *config);
 
 	char *name;
-	unsigned char type;
-	unsigned char id;
-	unsigned char read_reg;
-	unsigned int read_len;
-	unsigned char endian;
+	__u8 type;
+	__u8 id;
+	__u8 read_reg;
+	__u8 read_len;
+	__u8 endian;
 	struct fix_pnt_range range;
 	struct ext_slave_read_trigger *trigger;
 };
@@ -277,11 +328,12 @@ struct ext_slave_descr {
 /**
  * struct mpu_platform_data - Platform data for the mpu driver
  * @int_config:		Bits [7:3] of the int config register.
- * @orientation:	Orientation matrix of the gyroscope
  * @level_shifter:	0: VLogic, 1: VDD
- * @accel:		Accel platform data
- * @compass:		Compass platform data
- * @pressure:		Pressure platform data
+ * @orientation:	Orientation matrix of the gyroscope
+ * @sec_slave_type:     secondary slave device type, can be compass, accel, etc
+ * @sec_slave_id:       id of the secondary slave device
+ * @secondary_i2c_address: secondary device's i2c address
+ * @secondary_orientation: secondary device's orientation matrix
  *
  * Contains platform specific information on how to configure the MPU3050 to
  * work on this platform.  The orientation matricies are 3x3 rotation matricies
@@ -290,46 +342,14 @@ struct ext_slave_descr {
  * column should have exactly 1 non-zero value.
  */
 struct mpu_platform_data {
-	unsigned char int_config;
-	signed char orientation[GYRO_NUM_AXES * GYRO_NUM_AXES];
-	unsigned char level_shifter;
-	struct ext_slave_platform_data accel;
-	struct ext_slave_platform_data compass;
-	struct ext_slave_platform_data pressure;
+	__u8 int_config;
+	__u8 level_shifter;
+	__s8 orientation[9];
+	enum secondary_slave_type sec_slave_type;
+	enum ext_slave_id sec_slave_id;
+	__u16 secondary_i2c_addr;
+	__s8 secondary_orientation[9];
+	__u8 key[16];
 };
 
-#if defined __KERNEL__ || defined LINUX
-#define MPU_IOCTL (0x81) /* Magic number for MPU Iocts */
-/* IOCTL commands for /dev/mpu */
-#define MPU_SET_MPU_CONFIG	_IOWR(MPU_IOCTL, 0x00, struct mldl_cfg)
-#define MPU_GET_MPU_CONFIG	_IOW(MPU_IOCTL,  0x00, struct mldl_cfg)
-
-#define MPU_SET_PLATFORM_DATA	_IOWR(MPU_IOCTL, 0x01, struct mldl_cfg)
-
-#define MPU_READ		_IOWR(MPU_IOCTL, 0x10, struct mpu_read_write)
-#define MPU_WRITE		_IOW(MPU_IOCTL,  0x10, struct mpu_read_write)
-#define MPU_READ_MEM		_IOWR(MPU_IOCTL, 0x11, struct mpu_read_write)
-#define MPU_WRITE_MEM		_IOW(MPU_IOCTL,  0x11, struct mpu_read_write)
-#define MPU_READ_FIFO		_IOWR(MPU_IOCTL, 0x12, struct mpu_read_write)
-#define MPU_WRITE_FIFO		_IOW(MPU_IOCTL,  0x12, struct mpu_read_write)
-
-#define MPU_READ_COMPASS	_IOR(MPU_IOCTL, 0x12, unsigned char)
-#define MPU_READ_ACCEL		_IOR(MPU_IOCTL, 0x13, unsigned char)
-#define MPU_READ_PRESSURE	_IOR(MPU_IOCTL, 0x14, unsigned char)
-
-#define MPU_CONFIG_ACCEL	_IOW(MPU_IOCTL, 0x20, struct ext_slave_config)
-#define MPU_CONFIG_COMPASS	_IOW(MPU_IOCTL, 0x21, struct ext_slave_config)
-#define MPU_CONFIG_PRESSURE	_IOW(MPU_IOCTL, 0x22, struct ext_slave_config)
-
-#define MPU_GET_CONFIG_ACCEL	_IOWR(MPU_IOCTL, 0x20, struct ext_slave_config)
-#define MPU_GET_CONFIG_COMPASS	_IOWR(MPU_IOCTL, 0x21, struct ext_slave_config)
-#define MPU_GET_CONFIG_PRESSURE	_IOWR(MPU_IOCTL, 0x22, struct ext_slave_config)
-
-#define MPU_SUSPEND		_IO(MPU_IOCTL, 0x30)
-#define MPU_RESUME		_IO(MPU_IOCTL, 0x31)
-/* Userspace PM Event response */
-#define MPU_PM_EVENT_HANDLED	_IO(MPU_IOCTL, 0x32)
-
-#endif
-
-#endif				/* __MPU_H_ */
+#endif	/* __MPU_H_ */
diff --git a/mlsdk/platform/include/log.h b/libsensors/software/core/driver/include/log.h
index 8485074..74c49f3 100644
--- a/mlsdk/platform/include/log.h
+++ b/libsensors/software/core/driver/include/log.h
@@ -1,19 +1,7 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 
 /*
@@ -49,8 +37,8 @@
 #ifndef _LIBS_CUTILS_MPL_LOG_H
 #define _LIBS_CUTILS_MPL_LOG_H
 
-#include "mltypes.h"
 #include <stdarg.h>
+#include "local_log_def.h"
 
 #ifdef ANDROID
 #ifdef NDK_BUILD
@@ -127,11 +115,22 @@ extern "C" {
  */
 #ifndef MPL_LOGV
 #if MPL_LOG_NDEBUG
+#ifdef _WIN32
 #define MPL_LOGV(fmt, ...)						\
 	do {								\
+        __pragma (warning(suppress : 4127 )) \
 		if (0)							\
 			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
-	} while (0)
+            __pragma (warning(suppress : 4127 )) \
+    } while (0)
+#else
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+    } while (0)
+#endif
+
 #else
 #define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
 #endif
@@ -171,8 +170,12 @@ extern "C" {
  * Simplified macro to send an info log message using the current MPL_LOG_TAG.
  */
 #ifndef MPL_LOGI
+#ifdef __KERNEL__
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
 #define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
 #endif
+#endif
 
 #ifndef MPL_LOGI_IF
 #define MPL_LOGI_IF(cond, fmt, ...) \
@@ -284,7 +287,7 @@ extern "C" {
 #ifndef MPL_LOG_PRI
 #ifdef ANDROID
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
-        ALOG(priority, tag, fmt, ##__VA_ARGS__)
+	LOG(priority, tag, fmt, ##__VA_ARGS__)
 #elif defined __KERNEL__
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
 	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
@@ -331,12 +334,28 @@ static inline void __print_result_location(int result,
 	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
 }
 
+#ifdef _WIN32
+/* The pragma removes warning about expression being constant */
 #define LOG_RESULT_LOCATION(condition) \
-	do {								\
+    do {								\
 		__print_result_location((int)(condition), __FILE__,	\
 					__func__, __LINE__);		\
+        __pragma (warning(suppress : 4127 )) \
 	} while (0)
+#else
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+#endif
+
 
+#define INV_ERROR_CHECK(r_1329) \
+    if (r_1329) { \

+        LOG_RESULT_LOCATION(r_1329); \

+        return r_1329; \

+    }
 
 #ifdef __cplusplus
 }
diff --git a/mlsdk/mllite/mlinclude.h b/libsensors/software/core/driver/include/mlinclude.h
index dcbe904..9725199 100644
--- a/mlsdk/mllite/mlinclude.h
+++ b/libsensors/software/core/driver/include/mlinclude.h
@@ -1,19 +1,7 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 #ifndef INV_INCLUDE_H__
 #define INV_INCLUDE_H__
diff --git a/mlsdk/platform/include/mlmath.h b/libsensors/software/core/driver/include/mlmath.h
index bf54870..37194d6 100644
--- a/mlsdk/platform/include/mlmath.h
+++ b/libsensors/software/core/driver/include/mlmath.h
@@ -1,19 +1,7 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 /*******************************************************************************
  *
diff --git a/mlsdk/platform/include/mlsl.h b/libsensors/software/core/driver/include/mlsl.h
index 535d117..12f2901 100644
--- a/mlsdk/platform/include/mlsl.h
+++ b/libsensors/software/core/driver/include/mlsl.h
@@ -1,24 +1,16 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 
 #ifndef __MLSL_H__
 #define __MLSL_H__
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /**
  *  @defgroup   MLSL
  *  @brief      Motion Library - Serial Layer.
@@ -51,19 +43,12 @@
  *
  */
 
-#include "mltypes.h"
-#include <linux/mpu.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 /*
  * NOTE : to properly support Yamaha compass reads,
  *	  the max transfer size should be at least 9 B.
  *	  Length in bytes, typically a power of 2 >= 2
  */
-#define SERIAL_MAX_TRANSFER_SIZE 128
+#define SERIAL_MAX_TRANSFER_SIZE 31
 
 #ifndef __KERNEL__
 /**
@@ -80,7 +65,7 @@ extern "C" {
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_open(char const *port, void **sl_handle);
+int inv_serial_open(char const *port, void **sl_handle);
 
 /**
  *  inv_serial_close() - used to close the serial port.
@@ -95,13 +80,13 @@ inv_error_t inv_serial_open(char const *port, void **sl_handle);
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_close(void *sl_handle);
+int inv_serial_close(void *sl_handle);
 
 /**
  *  inv_serial_reset() - used to reset any buffering the driver may be doing
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_reset(void *sl_handle);
+int inv_serial_reset(void *sl_handle);
 #endif
 
 /**
@@ -115,7 +100,7 @@ inv_error_t inv_serial_reset(void *sl_handle);
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_single_write(
+int inv_serial_single_write(
 	void *sl_handle,
 	unsigned char slave_addr,
 	unsigned char register_addr,
@@ -131,7 +116,7 @@ inv_error_t inv_serial_single_write(
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_write(
+int inv_serial_write(
 	void *sl_handle,
 	unsigned char slave_addr,
 	unsigned short length,
@@ -147,7 +132,7 @@ inv_error_t inv_serial_write(
  *
  *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
  */
-inv_error_t inv_serial_read(
+int inv_serial_read(
 	void *sl_handle,
 	unsigned char slave_addr,
 	unsigned char register_addr,
@@ -166,10 +151,13 @@ inv_error_t inv_serial_read(
  *
  *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
  */
-inv_error_t inv_serial_read_mem(
+int inv_serial_read_mem(
 	void *sl_handle,
 	unsigned char slave_addr,
 	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
 	unsigned short length,
 	unsigned char *data);
 
@@ -183,12 +171,15 @@ inv_error_t inv_serial_read_mem(
  *
  *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
  */
-inv_error_t inv_serial_write_mem(
+int inv_serial_write_mem(
 	void *sl_handle,
 	unsigned char slave_addr,
 	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
 	unsigned short length,
-	unsigned char const *data);
+	unsigned char *data);
 
 /**
  *  inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
@@ -199,9 +190,10 @@ inv_error_t inv_serial_write_mem(
  *
  *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
  */
-inv_error_t inv_serial_read_fifo(
+int inv_serial_read_fifo(
 	void *sl_handle,
 	unsigned char slave_addr,
+	unsigned char fifo_reg,
 	unsigned short length,
 	unsigned char *data);
 
@@ -214,9 +206,10 @@ inv_error_t inv_serial_read_fifo(
  *
  *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
  */
-inv_error_t inv_serial_write_fifo(
+int inv_serial_write_fifo(
 	void *sl_handle,
 	unsigned char slave_addr,
+	unsigned char fifo_reg,
 	unsigned short length,
 	unsigned char const *data);
 
@@ -232,7 +225,7 @@ inv_error_t inv_serial_write_fifo(
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
+int inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
 
 /**
  *  inv_serial_write_cfg() - used to save the configuration data.
@@ -245,7 +238,7 @@ inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
+int inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
 
 /**
  *  inv_serial_read_cal() - used to get the calibration data.
@@ -258,7 +251,7 @@ inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len);
+int inv_serial_read_cal(unsigned char *cal, unsigned int len);
 
 /**
  *  inv_serial_write_cal() - used to save the calibration data.
@@ -271,7 +264,7 @@ inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len);
  *	    This data is typically be saved in non-volatile memory.
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len);
+int inv_serial_write_cal(unsigned char *cal, unsigned int len);
 
 /**
  *  inv_serial_get_cal_length() - Get the calibration length from the storage.
@@ -279,7 +272,7 @@ inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len);
  *
  *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
  */
-inv_error_t inv_serial_get_cal_length(unsigned int *len);
+int inv_serial_get_cal_length(unsigned int *len);
 #endif
 #ifdef __cplusplus
 }
diff --git a/libsensors/software/core/driver/include/mltypes.h b/libsensors/software/core/driver/include/mltypes.h
new file mode 100644
index 0000000..09eccce
--- /dev/null
+++ b/libsensors/software/core/driver/include/mltypes.h
@@ -0,0 +1,235 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Motion Library - Error definitions.
+ *          Definition of the error codes used within the MPL and
+ *          returned to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)               INV_SUCCESS
+ *          - (32)              INV_ERROR
+ *          - (22 / EINVAL)     INV_ERROR_INVALID_PARAMETER
+ *          - (1  / EPERM)      INV_ERROR_FEATURE_NOT_ENABLED
+ *          - (36)              INV_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (38)              INV_ERROR_DMP_NOT_STARTED
+ *          - (39)              INV_ERROR_DMP_STARTED
+ *          - (40)              INV_ERROR_NOT_OPENED
+ *          - (41)              INV_ERROR_OPENED
+ *          - (19 / ENODEV)     INV_ERROR_INVALID_MODULE
+ *          - (12 / ENOMEM)     INV_ERROR_MEMORY_EXAUSTED
+ *          - (44)              INV_ERROR_DIVIDE_BY_ZERO
+ *          - (45)              INV_ERROR_ASSERTION_FAILURE
+ *          - (46)              INV_ERROR_FILE_OPEN
+ *          - (47)              INV_ERROR_FILE_READ
+ *          - (48)              INV_ERROR_FILE_WRITE
+ *          - (49)              INV_ERROR_INVALID_CONFIGURATION
+ *          - (52)              INV_ERROR_SERIAL_CLOSED
+ *          - (53)              INV_ERROR_SERIAL_OPEN_ERROR
+ *          - (54)              INV_ERROR_SERIAL_READ
+ *          - (55)              INV_ERROR_SERIAL_WRITE
+ *          - (56)              INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (57)              INV_ERROR_SM_TRANSITION
+ *          - (58)              INV_ERROR_SM_IMPROPER_STATE
+ *          - (62)              INV_ERROR_FIFO_OVERFLOW
+ *          - (63)              INV_ERROR_FIFO_FOOTER
+ *          - (64)              INV_ERROR_FIFO_READ_COUNT
+ *          - (65)              INV_ERROR_FIFO_READ_DATA
+ *          - (72)              INV_ERROR_MEMORY_SET
+ *          - (82)              INV_ERROR_LOG_MEMORY_ERROR
+ *          - (83)              INV_ERROR_LOG_OUTPUT_ERROR
+ *          - (92)              INV_ERROR_OS_BAD_PTR
+ *          - (93)              INV_ERROR_OS_BAD_HANDLE
+ *          - (94)              INV_ERROR_OS_CREATE_FAILED
+ *          - (95)              INV_ERROR_OS_LOCK_FAILED
+ *          - (102)             INV_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (103)             INV_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (104)             INV_ERROR_COMPASS_DATA_NOT_READY
+ *          - (105)             INV_ERROR_COMPASS_DATA_ERROR
+ *          - (107)             INV_ERROR_CALIBRATION_LOAD
+ *          - (108)             INV_ERROR_CALIBRATION_STORE
+ *          - (109)             INV_ERROR_CALIBRATION_LEN
+ *          - (110)             INV_ERROR_CALIBRATION_CHECKSUM
+ *          - (111)             INV_ERROR_ACCEL_DATA_OVERFLOW
+ *          - (112)             INV_ERROR_ACCEL_DATA_UNDERFLOW
+ *          - (113)             INV_ERROR_ACCEL_DATA_NOT_READY
+ *          - (114)             INV_ERROR_ACCEL_DATA_ERROR
+ *
+ *          The available warning codes and their associated values are:
+ *          - (115)             INV_WARNING_MOTION_RACE
+ *          - (116)             INV_WARNING_QUAT_TRASHED
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#include <asm-generic/errno-base.h>
+#else
+#include "stdint_invensense.h"
+#include <errno.h>
+#endif
+#include <limits.h>
+
+#ifndef REMOVE_INV_ERROR_T
+/*---------------------------
+ *    ML Types
+ *--------------------------*/
+
+/**
+ *  @struct inv_error_t mltypes.h "mltypes"
+ *  @brief  The MPL Error Code return type.
+ *
+ *  @code
+ *      typedef unsigned char inv_error_t;
+ *  @endcode
+ */
+//typedef unsigned char inv_error_t;
+typedef int inv_error_t;
+#endif
+
+typedef long long inv_time_t;
+
+#if !defined __GNUC__ && !defined __KERNEL__
+typedef int8_t   __s8;
+typedef int16_t  __s16;
+typedef int32_t  __s32;
+typedef int32_t  __s64;
+
+typedef uint8_t   __u8;
+typedef uint16_t  __u16;
+typedef uint32_t  __u32;
+typedef uint64_t  __u64;
+#elif !defined __KERNEL__
+#include <sys/types.h>
+#endif
+
+#ifndef __cplusplus
+#ifndef __KERNEL__
+typedef int_fast8_t bool;
+
+#ifndef false
+#define false 0
+#endif
+
+#ifndef true
+#define true 1
+#endif
+
+#endif
+#endif
+
+/*---------------------------
+ *    ML Defines
+ *--------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifndef __KERNEL__
+#ifndef ARRAY_SIZE
+/* Dimension of an array */
+#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
+#endif
+#endif
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK_FIRST(first, x) \
+	{ if (INV_SUCCESS == first) first = x; }
+
+#define INV_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define INV_ERROR_BASE                    (0x20)
+#define INV_ERROR                         (INV_ERROR_BASE)
+
+/* Compatibility and other generic error codes */
+#define INV_ERROR_INVALID_PARAMETER             (EINVAL)
+#define INV_ERROR_FEATURE_NOT_ENABLED           (EPERM)
+#define INV_ERROR_FEATURE_NOT_IMPLEMENTED       (INV_ERROR_BASE + 4)
+#define INV_ERROR_DMP_NOT_STARTED               (INV_ERROR_BASE + 6)
+#define INV_ERROR_DMP_STARTED                   (INV_ERROR_BASE + 7)
+#define INV_ERROR_NOT_OPENED                    (INV_ERROR_BASE + 8)
+#define INV_ERROR_OPENED                        (INV_ERROR_BASE + 9)
+#define INV_ERROR_INVALID_MODULE                (ENODEV)
+#define INV_ERROR_MEMORY_EXAUSTED               (ENOMEM)
+#define INV_ERROR_DIVIDE_BY_ZERO                (INV_ERROR_BASE + 12)
+#define INV_ERROR_ASSERTION_FAILURE             (INV_ERROR_BASE + 13)
+#define INV_ERROR_FILE_OPEN                     (INV_ERROR_BASE + 14)
+#define INV_ERROR_FILE_READ                     (INV_ERROR_BASE + 15)
+#define INV_ERROR_FILE_WRITE                    (INV_ERROR_BASE + 16)
+#define INV_ERROR_INVALID_CONFIGURATION         (INV_ERROR_BASE + 17)
+#define INV_ERROR_NOT_AUTHORIZED                (INV_ERROR_BASE + 18)
+
+/* Serial Communication */
+#define INV_ERROR_SERIAL_CLOSED                 (INV_ERROR_BASE + 20)
+#define INV_ERROR_SERIAL_OPEN_ERROR             (INV_ERROR_BASE + 21)
+#define INV_ERROR_SERIAL_READ                   (INV_ERROR_BASE + 22)
+#define INV_ERROR_SERIAL_WRITE                  (INV_ERROR_BASE + 23)
+#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (INV_ERROR_BASE + 24)
+
+/* SM = State Machine */
+#define INV_ERROR_SM_TRANSITION                 (INV_ERROR_BASE + 25)
+#define INV_ERROR_SM_IMPROPER_STATE             (INV_ERROR_BASE + 26)
+
+/* Fifo */
+#define INV_ERROR_FIFO_OVERFLOW                 (INV_ERROR_BASE + 30)
+#define INV_ERROR_FIFO_FOOTER                   (INV_ERROR_BASE + 31)
+#define INV_ERROR_FIFO_READ_COUNT               (INV_ERROR_BASE + 32)
+#define INV_ERROR_FIFO_READ_DATA                (INV_ERROR_BASE + 33)
+
+/* Memory & Registers, Set & Get */
+#define INV_ERROR_MEMORY_SET                    (INV_ERROR_BASE + 40)
+
+#define INV_ERROR_LOG_MEMORY_ERROR              (INV_ERROR_BASE + 50)
+#define INV_ERROR_LOG_OUTPUT_ERROR              (INV_ERROR_BASE + 51)
+
+/* OS interface errors */
+#define INV_ERROR_OS_BAD_PTR                    (INV_ERROR_BASE + 60)
+#define INV_ERROR_OS_BAD_HANDLE                 (INV_ERROR_BASE + 61)
+#define INV_ERROR_OS_CREATE_FAILED              (INV_ERROR_BASE + 62)
+#define INV_ERROR_OS_LOCK_FAILED                (INV_ERROR_BASE + 63)
+
+/* Compass errors */
+#define INV_ERROR_COMPASS_DATA_OVERFLOW         (INV_ERROR_BASE + 70)
+#define INV_ERROR_COMPASS_DATA_UNDERFLOW        (INV_ERROR_BASE + 71)
+#define INV_ERROR_COMPASS_DATA_NOT_READY        (INV_ERROR_BASE + 72)
+#define INV_ERROR_COMPASS_DATA_ERROR            (INV_ERROR_BASE + 73)
+
+/* Load/Store calibration */
+#define INV_ERROR_CALIBRATION_LOAD              (INV_ERROR_BASE + 75)
+#define INV_ERROR_CALIBRATION_STORE             (INV_ERROR_BASE + 76)
+#define INV_ERROR_CALIBRATION_LEN               (INV_ERROR_BASE + 77)
+#define INV_ERROR_CALIBRATION_CHECKSUM          (INV_ERROR_BASE + 78)
+
+/* Accel errors */
+#define INV_ERROR_ACCEL_DATA_OVERFLOW           (INV_ERROR_BASE + 79)
+#define INV_ERROR_ACCEL_DATA_UNDERFLOW          (INV_ERROR_BASE + 80)
+#define INV_ERROR_ACCEL_DATA_NOT_READY          (INV_ERROR_BASE + 81)
+#define INV_ERROR_ACCEL_DATA_ERROR              (INV_ERROR_BASE + 82)
+
+/* No Motion Warning States */
+#define INV_WARNING_MOTION_RACE                 (INV_ERROR_BASE + 83)
+#define INV_WARNING_QUAT_TRASHED                (INV_ERROR_BASE + 84)
+#define INV_WARNING_GYRO_MAG                    (INV_ERROR_BASE + 85)
+
+#define INV_WARNING_SEMAPHORE_TIMEOUT           (INV_ERROR_BASE + 86)
+
+
+/* For Linux coding compliance */
+#ifndef __KERNEL__
+#define EXPORT_SYMBOL(x)
+#endif
+
+#endif				/* MLTYPES_H */
diff --git a/libsensors/software/core/driver/include/stdint_invensense.h b/libsensors/software/core/driver/include/stdint_invensense.h
new file mode 100644
index 0000000..b8c2511
--- /dev/null
+++ b/libsensors/software/core/driver/include/stdint_invensense.h
@@ -0,0 +1,41 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef STDINT_INVENSENSE_H
+#define STDINT_INVENSENSE_H
+
+#ifndef WIN32
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#endif
+
+#else
+
+#include <windows.h>
+
+typedef signed char int8_t;
+typedef short int16_t;
+typedef long int32_t;
+typedef long long int64_t;
+
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned long uint32_t;
+typedef unsigned long long uint64_t;
+
+typedef int int_fast8_t;
+typedef int int_fast16_t;
+typedef long int_fast32_t;
+
+typedef unsigned int uint_fast8_t;
+typedef unsigned int uint_fast16_t;
+typedef unsigned long uint_fast32_t;
+
+#endif
+
+#endif // STDINT_INVENSENSE_H
diff --git a/libsensors/software/core/mllite/CMakeLists.txt b/libsensors/software/core/mllite/CMakeLists.txt
new file mode 100644
index 0000000..8650553
--- /dev/null
+++ b/libsensors/software/core/mllite/CMakeLists.txt
@@ -0,0 +1,39 @@
+## 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/software/core/mllite/Engineering.cmake b/libsensors/software/core/mllite/Engineering.cmake
new file mode 100644
index 0000000..42f2429
--- /dev/null
+++ b/libsensors/software/core/mllite/Engineering.cmake
@@ -0,0 +1,150 @@
+## 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/software/core/mllite/build/android/shared.mk b/libsensors/software/core/mllite/build/android/shared.mk
new file mode 100644
index 0000000..2ee2e20
--- /dev/null
+++ b/libsensors/software/core/mllite/build/android/shared.mk
@@ -0,0 +1,91 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc 
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(INV_ROOT)/simple_apps/common
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
+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
+
+####################################################################################################
+## sources
+
+#INV_SOURCES provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all mllite clean cleanall makefiles
+
+all: mllite
+
+mllite: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors/software/core/mllite/build/android/static.mk b/libsensors/software/core/mllite/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors/software/core/mllite/build/android/static.mk
@@ -0,0 +1,110 @@
+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/software/core/mllite/build/filelist.mk b/libsensors/software/core/mllite/build/filelist.mk
new file mode 100644
index 0000000..011120c
--- /dev/null
+++ b/libsensors/software/core/mllite/build/filelist.mk
@@ -0,0 +1,42 @@
+#### filelist.mk for mllite ####
+
+# headers only
+HEADERS := $(MLLITE_DIR)/invensense.h
+
+# headers for sources
+HEADERS += $(MLLITE_DIR)/data_builder.h
+HEADERS += $(MLLITE_DIR)/hal_outputs.h
+HEADERS += $(MLLITE_DIR)/message_layer.h
+HEADERS += $(MLLITE_DIR)/ml_math_func.h
+HEADERS += $(MLLITE_DIR)/mpl.h
+HEADERS += $(MLLITE_DIR)/results_holder.h
+HEADERS += $(MLLITE_DIR)/start_manager.h
+HEADERS += $(MLLITE_DIR)/storage_manager.h
+
+# headers (linux specific)
+HEADERS += $(MLLITE_DIR)/linux/mlos.h
+HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h
+HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h
+HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h
+
+# sources
+SOURCES := $(MLLITE_DIR)/data_builder.c
+SOURCES += $(MLLITE_DIR)/hal_outputs.c
+SOURCES += $(MLLITE_DIR)/message_layer.c
+SOURCES += $(MLLITE_DIR)/ml_math_func.c
+SOURCES += $(MLLITE_DIR)/mpl.c
+SOURCES += $(MLLITE_DIR)/results_holder.c
+SOURCES += $(MLLITE_DIR)/start_manager.c
+SOURCES += $(MLLITE_DIR)/storage_manager.c
+
+# sources (linux specific)
+SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c
+SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c
+SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c
+SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c
+
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux
+
diff --git a/libsensors/software/core/mllite/data_builder.c b/libsensors/software/core/mllite/data_builder.c
new file mode 100644
index 0000000..f70be7c
--- /dev/null
+++ b/libsensors/software/core/mllite/data_builder.c
@@ -0,0 +1,1162 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+ 
+/**
+ *   @defgroup  Data_Builder data_builder
+ *   @brief     Motion Library - Data Builder
+ *              Constructs and Creates the data for MPL
+ *
+ *   @{
+ *       @file data_builder.c
+ *       @brief Data Builder.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+
+#include "ml_math_func.h"
+#include "data_builder.h"
+#include "mlmath.h"
+#include "storage_manager.h"
+#include "message_layer.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL"
+
+typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data);
+
+struct process_t {
+    inv_process_cb_func func;
+    int priority;
+    int data_required;
+};
+
+struct inv_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+};
+
+struct inv_data_builder_t {
+    int num_cb;
+    struct process_t process[INV_MAX_DATA_CB];
+    struct inv_db_save_t save;
+    int compass_disturbance;
+#ifdef INV_PLAYBACK_DBG
+    int debug_mode;
+    int last_mode;
+    FILE *file;
+#endif
+};
+
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias);
+static void inv_set_contiguous(void);
+
+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
+
+#ifdef INV_PLAYBACK_DBG
+
+/** Turn on data logging to allow playback of same scenario at a later time.
+* @param[in] file File to write to, must be open.
+*/
+void inv_turn_on_data_logging(FILE *file)
+{
+    MPL_LOGV("input data logging started\n");
+    inv_data_builder.file = file;
+    inv_data_builder.debug_mode = RD_RECORD;
+}
+
+/** Turn off data logging to allow playback of same scenario at a later time.
+* File passed to inv_turn_on_data_logging() must be closed after calling this.
+*/
+void inv_turn_off_data_logging()
+{
+    MPL_LOGV("input data logging stopped\n");
+    inv_data_builder.debug_mode = RD_NO_DEBUG;
+    inv_data_builder.file = NULL;
+}
+#endif
+
+/** 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(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save));
+    return INV_SUCCESS;
+}
+
+/** Initialize the data builder
+*/
+inv_error_t inv_init_data_builder(void)
+{
+    /* TODO: Hardcode temperature scale/offset here. */
+    memset(&inv_data_builder, 0, sizeof(inv_data_builder));
+    memset(&sensors, 0, sizeof(sensors));
+    return inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                   sizeof(inv_data_builder.save),
+                                   INV_DB_SAVE_KEY);
+}
+
+/** Gyro sensitivity.
+* @return A scale factor to convert device units to degrees per second scaled by 2^16
+* such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum rate * 2^15.
+*/
+long inv_get_gyro_sensitivity()
+{
+    return sensors.gyro.sensitivity;
+}
+
+/** Accel sensitivity.
+* @return A scale factor to convert device units to g's scaled by 2^16
+* such that g_s  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum accel value in g's * 2^15.
+*/
+long inv_get_accel_sensitivity(void)
+{
+    return sensors.accel.sensitivity;
+}
+
+/** Compass sensitivity.
+* @return A scale factor to convert device units to micro Tesla scaled by 2^16
+* such that uT  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum uT * 2^15.
+*/
+long inv_get_compass_sensitivity(void)
+{
+    return sensors.compass.sensitivity;
+}
+
+/** Sets orientation and sensitivity field for a sensor.
+* @param[out] sensor Structure to apply settings to
+* @param[in] orientation Orientation description of how part is mounted.
+* @param[in] sensitivity A Scale factor to convert from hardware units to
+*            standard units (dps, uT, g).
+*/
+void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
+                                 int orientation, long sensitivity)
+{
+    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()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16
+*            such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum rate * 2^15.
+*/
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.gyro, orientation,
+                                     sensitivity);
+}
+
+/** Set Gyro Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in us
+*/
+void inv_set_gyro_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.sample_rate_us = sample_rate_us;
+    sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.gyro.bandwidth == 0) {
+        sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Accel Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Accel Sample rate in us
+*/
+void inv_set_accel_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.sample_rate_us = sample_rate_us;
+    sensors.accel.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.accel.bandwidth == 0) {
+        sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Compass Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds.
+*/
+void inv_set_compass_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.sample_rate_us = sample_rate_us;
+    sensors.compass.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.compass.bandwidth == 0) {
+        sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+/** Set Quat Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Quat Sample rate in us
+*/
+void inv_set_quat_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.sample_rate_us = sample_rate_us;
+    sensors.quat.sample_rate_ms = sample_rate_us / 1000;
+}
+
+/** Set Gyro Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_gyro_bandwidth(int bandwidth_hz)
+{
+    sensors.gyro.bandwidth = bandwidth_hz;
+}
+
+/** Set Accel Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_accel_bandwidth(int bandwidth_hz)
+{
+    sensors.accel.bandwidth = bandwidth_hz;
+}
+
+/** Set Compass Bandwidth in Hz
+* @param[in]  bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_compass_bandwidth(int bandwidth_hz)
+{
+    sensors.compass.bandwidth = bandwidth_hz;
+}
+
+/** Helper function stating whether the compass is on or off.
+ * @return TRUE if compass if on, 0 if compass if off
+*/
+int inv_get_compass_on()
+{
+    return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the gyro is on or off.
+ * @return TRUE if gyro if on, 0 if gyro if off
+*/
+int inv_get_gyro_on()
+{
+    return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the acceleromter is on or off.
+ * @return TRUE if accel if on, 0 if accel if off
+*/
+int inv_get_accel_on()
+{
+    return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Get last timestamp across all 3 sensors that are on.
+* This find out which timestamp has the largest value for sensors that are on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_time_t inv_get_last_timestamp()
+{
+    inv_time_t timestamp = 0;
+    if (sensors.accel.status & INV_SENSOR_ON) {
+        timestamp = sensors.accel.timestamp;
+    }
+    if (sensors.gyro.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.gyro.timestamp) {
+            timestamp = sensors.gyro.timestamp;
+        }
+    }
+    if (sensors.compass.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.compass.timestamp) {
+            timestamp = sensors.compass.timestamp;
+        }
+    }
+    if (sensors.temp.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.temp.timestamp)
+            timestamp = sensors.temp.timestamp;
+    }
+    return timestamp;
+}
+
+/** 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()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to g's
+*            such that g's = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum g_value * 2^15.
+*/
+void inv_set_accel_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.accel, orientation,
+                                     sensitivity);
+}
+
+/** 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()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to uT
+*            such that uT = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum uT_value * 2^15.
+*/
+void inv_set_compass_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
+}
+
+void inv_matrix_vector_mult(const long *A, const long *x, long *y)
+{
+    y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
+    y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
+    y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
+}
+
+/** Takes raw data stored in the sensor, removes bias, and converts it to
+* calibrated data in the 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.
+*/
+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] -= bias[0];
+    raw32[1] -= bias[1];
+    raw32[2] -= bias[2];
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated);
+
+    sensor->status |= INV_CALIBRATED;
+}
+
+/** Returns the current bias for the compass
+* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame.
+*             Length 3.
+*/
+void inv_get_compass_bias(long *bias)
+{
+    if (bias != NULL) {
+        memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias));
+    }
+}
+
+void inv_set_compass_bias(const long *bias, int accuracy)
+{
+    if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) {
+        memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias));
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+    }
+    sensors.compass.accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
+}
+
+/** Set the state of a compass disturbance
+* @param[in] dist 1=disturbance, 0=no disturbance
+*/
+void inv_set_compass_disturbance(int dist)
+{
+    inv_data_builder.compass_disturbance = dist;
+}
+
+int inv_get_compass_disturbance(void) {
+    return inv_data_builder.compass_disturbance;
+}
+/** Sets the accel bias.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_bias(const long *bias, int accuracy)
+{
+    if (bias) {
+        if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) {
+            memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias));
+            inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+        }
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel bias with control over which axis.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+* @param[in] mask Mask to select axis to apply bias set.
+*/
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
+{
+    if (bias) {
+        if (mask & 1){
+            inv_data_builder.save.accel_bias[0] = bias[0];
+        }
+        if (mask & 2){
+            inv_data_builder.save.accel_bias[1] = bias[1];
+        }
+        if (mask & 4){
+            inv_data_builder.save.accel_bias[2] = bias[2];
+        }
+
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    }
+    sensors.accel.accuracy = accuracy;
+}
+
+
+/** Sets the gyro bias
+* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame.
+*            Length 3.
+* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate.
+*/
+void inv_set_gyro_bias(const long *bias, int accuracy)
+{
+    if (bias != NULL) {
+        if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) {
+            memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias));
+            inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
+        }
+    }
+    sensors.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];
+    else
+        /* Set to 27 deg C for now until we've got a better solution. */
+        inv_data_builder.save.gyro_temp = 1769472L;
+    inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
+}
+
+/* TODO: Add this information to inv_sensor_cal_t */
+/** 
+ *  Get the gyro biases and temperature record from MPL 
+ *  @param[in] bias 
+ *              Gyro bias in hardware units scaled by 2^16. 
+ *              In chip mounting frame.
+ *              Length 3.
+ *  @param[in] temp
+ *              Tempearature in degrees C. 
+ */
+void inv_get_gyro_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.gyro_bias,
+               sizeof(inv_data_builder.save.gyro_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.gyro_temp;
+}
+
+/** Get Accel Bias
+* @param[out] bias Accel bias where
+* @param[out] temp Temperature where 1 C = 2^16
+*/
+void inv_get_accel_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.accel_bias,
+               sizeof(inv_data_builder.save.accel_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.accel_temp;
+}
+
+/** 
+ *  Record new accel data for use when inv_execute_on_data() is called
+ *  @param[in]  accel accel data. 
+ *              Length 3. 
+ *              Calibrated data is in m/s^2 scaled by 2^16 in body frame. 
+ *              Raw data is in device units in chip mounting frame.
+ *  @param[in]  status 
+ *              Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 
+ *              being most accurate.
+ *              The upper bit INV_CALIBRATED, is set if the data was calibrated 
+ *              outside MPL and it is not set if the data being passed is raw. 
+ *              Raw data should be in device units, typically in a 16-bit range.
+ *  @param[in]  timestamp 
+ *              Monotonic time stamp, for Android it's in nanoseconds.
+ *  @return     Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_ACCEL;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.accel.raw[0] = (short)accel[0];
+        sensors.accel.raw[1] = (short)accel[1];
+        sensors.accel.raw[2] = (short)accel[2];
+        sensors.accel.status |= INV_RAW_DATA;
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    } else {
+        sensors.accel.calibrated[0] = accel[0];
+        sensors.accel.calibrated[1] = accel[1];
+        sensors.accel.calibrated[2] = accel[2];
+        sensors.accel.status |= INV_CALIBRATED;
+        sensors.accel.accuracy = status & 3;
+    }
+    sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
+    sensors.accel.timestamp_prev = sensors.accel.timestamp;
+    sensors.accel.timestamp = timestamp;
+
+    return INV_SUCCESS;
+}
+
+/** Record new gyro data and calls inv_execute_on_data() if previous
+* sample has not been processed.
+* @param[in] gyro Data is in device units. Length 3.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
+    sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
+    sensors.gyro.timestamp = timestamp;
+    inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
+
+    return INV_SUCCESS;
+}
+
+/** Record new compass data for use when inv_execute_on_data() is called
+* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16.
+*            Length 3.
+* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate.
+*            The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is
+*            not set if the data being passed is raw. Raw data should be in device units, typically
+*            in a 16-bit range.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_compass(const long *compass, int status,
+                              inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.compass.raw[0] = (short)compass[0];
+        sensors.compass.raw[1] = (short)compass[1];
+        sensors.compass.raw[2] = (short)compass[2];
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+        sensors.compass.status |= INV_RAW_DATA;
+    } else {
+        sensors.compass.calibrated[0] = compass[0];
+        sensors.compass.calibrated[1] = compass[1];
+        sensors.compass.calibrated[2] = compass[2];
+        sensors.compass.status |= INV_CALIBRATED;
+        sensors.compass.accuracy = status & 3;
+    }
+    sensors.compass.timestamp_prev = sensors.compass.timestamp;
+    sensors.compass.timestamp = timestamp;
+    sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
+
+    return INV_SUCCESS;
+}
+
+/** Record new temperature data for use when inv_execute_on_data() is called.
+ *  @param[in]  temp Temperature data in q16 format.
+ *  @param[in]  timestamp   Monotonic time stamp; for Android it's in
+ *                          nanoseconds.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_TEMPERATURE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&temp, sizeof(temp), 1, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.temp.calibrated[0] = temp;
+    sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.temp.timestamp_prev = sensors.temp.timestamp;
+    sensors.temp.timestamp = timestamp;
+    /* TODO: Apply scale, remove offset. */
+
+    return INV_SUCCESS;
+}
+/** quaternion data
+* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 
+*                 Real part first. Length 4.  
+* @param[in] status number of axis, 16-bit or 32-bit
+* @param[in] timestamp
+* @param[in]  timestamp   Monotonic time stamp; for Android it's in
+*                         nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    
+    memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
+    sensors.quat.timestamp = timestamp;
+    sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.quat.status |= (INV_BIAS_APPLIED & status);
+
+    return INV_SUCCESS;
+}
+
+/** This should be called when the accel has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_accel_was_turned_off()
+{
+    sensors.accel.status = 0;
+}
+
+/** This should be called when the compass has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_compass_was_turned_off()
+{
+    sensors.compass.status = 0;
+}
+
+/** This should be called when the quaternion data from the DMP has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_quaternion_sensor_was_turned_off(void)
+{
+    sensors.quat.status = 0;
+}
+
+/** This should be called when the gyro has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_gyro_was_turned_off()
+{
+    sensors.gyro.status = 0;
+}
+
+/** This should be called when the temperature sensor has been turned off.
+ *  This is so that we will know if the data is contiguous.
+ */
+void inv_temperature_was_turned_off()
+{
+    sensors.temp.status = 0;
+}
+
+/** Registers to receive a callback when there is new sensor data.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_register_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data),
+    int priority, int sensor_type)
+{
+    inv_error_t result = INV_SUCCESS;
+    int kk, nn;
+
+    // Make sure we haven't registered this function already
+    // Or used the same priority
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if ((inv_data_builder.process[kk].func == func) ||
+                (inv_data_builder.process[kk].priority == priority)) {
+            return INV_ERROR_INVALID_PARAMETER;    //fixme give a warning
+        }
+    }
+
+    // Make sure we have not filled up our number of allowable callbacks
+    if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
+        kk = 0;
+        if (inv_data_builder.num_cb != 0) {
+            // set kk to be where this new callback goes in the array
+            while ((kk < inv_data_builder.num_cb) &&
+                    (inv_data_builder.process[kk].priority < priority)) {
+                kk++;
+            }
+            if (kk != inv_data_builder.num_cb) {
+                // We need to move the others
+                for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
+                    inv_data_builder.process[nn] =
+                        inv_data_builder.process[nn - 1];
+                }
+            }
+        }
+        // Add new callback
+        inv_data_builder.process[kk].func = func;
+        inv_data_builder.process[kk].priority = priority;
+        inv_data_builder.process[kk].data_required = sensor_type;
+        inv_data_builder.num_cb++;
+    } else {
+        MPL_LOGE("Unable to add feature callback as too many were already registered\n");
+        result = INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    return result;
+}
+
+/** Unregisters the callback that happens when new sensor data is received.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_unregister_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data))
+{
+    int kk, nn;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.process[kk].func == func) {
+            // Delete this callback
+            for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) {
+                inv_data_builder.process[nn - 1] =
+                    inv_data_builder.process[nn];
+            }
+            inv_data_builder.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+
+    return INV_SUCCESS;    // We did not find the callback
+}
+
+/** After at least one of inv_build_gyro(), inv_build_accel(), or
+* inv_build_compass() has been called, this function should be called.
+* It will process the data it has received and update all the internal states
+* and features that have been turned on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_on_data(void)
+{
+    inv_error_t result, first_error;
+    int kk;
+    int mode;
+
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_EXECUTE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    // Determine what new data we have
+    mode = 0;
+    if (sensors.gyro.status & INV_NEW_DATA)
+        mode |= INV_GYRO_NEW;
+    if (sensors.accel.status & INV_NEW_DATA)
+        mode |= INV_ACCEL_NEW;
+    if (sensors.compass.status & INV_NEW_DATA)
+        mode |= INV_MAG_NEW;
+    if (sensors.temp.status & INV_NEW_DATA)
+        mode |= INV_TEMP_NEW;
+    if (sensors.quat.status & INV_QUAT_NEW)
+        mode |= INV_QUAT_NEW;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (mode & inv_data_builder.process[kk].data_required) {
+            result = inv_data_builder.process[kk].func(&sensors);
+            if (result && !first_error) {
+                first_error = result;
+            }
+        }
+    }
+
+    inv_set_contiguous();
+
+    return first_error;
+}
+
+/** Cleans up status bits after running all the callbacks. It sets the contiguous flag.
+*
+*/
+static void inv_set_contiguous(void)
+{
+    inv_time_t current_time = 0;
+    if (sensors.gyro.status & INV_NEW_DATA) {
+        sensors.gyro.status |= INV_CONTIGUOUS;
+        current_time = sensors.gyro.timestamp;
+    }
+    if (sensors.accel.status & INV_NEW_DATA) {
+        sensors.accel.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.accel.timestamp);
+    }
+    if (sensors.compass.status & INV_NEW_DATA) {
+        sensors.compass.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.compass.timestamp);
+    }
+    if (sensors.temp.status & INV_NEW_DATA) {
+        sensors.temp.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.temp.timestamp);
+    }
+    if (sensors.quat.status & INV_NEW_DATA) {
+        sensors.quat.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.quat.timestamp);
+    }
+
+#if 0
+    /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
+     * type functions. This is just in case that breaks down. We make sure
+     * all the data is within 2 seconds of the newest piece of data*/
+    if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
+        inv_gyro_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
+        inv_accel_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
+        inv_compass_was_turned_off();
+    /* TODO: Temperature might not need to be read this quickly. */
+    if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
+        inv_temperature_was_turned_off();
+#endif
+
+    /* clear bits */
+    sensors.gyro.status &= ~INV_NEW_DATA;
+    sensors.accel.status &= ~INV_NEW_DATA;
+    sensors.compass.status &= ~INV_NEW_DATA;
+    sensors.temp.status &= ~INV_NEW_DATA;
+    sensors.quat.status &= ~INV_NEW_DATA;
+}
+
+/** Gets a whole set of accel data including data, accuracy and timestamp.
+ * @param[out] data Accel Data where 1g = 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_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    if (data != NULL) {
+        memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
+    }
+    if (timestamp != NULL) {
+        *timestamp = sensors.accel.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.accel.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro 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(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+    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.
+*/
+void inv_get_gyro(long *gyro)
+{
+    memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+}
+
+/** Gets a whole set of compass data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 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_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    if (accuracy != NULL) {
+        if (inv_data_builder.compass_disturbance)
+            *accuracy = 0;
+        else
+            *accuracy = sensors.compass.accuracy;
+    }
+}
+
+/** Gets a whole set of temperature data including data, accuracy and timestamp.
+ *  @param[out] data        Temperature data where 1 degree C = 2^16
+ *  @param[out] accuracy    0 to 3, where 3 is most accurate.
+ *  @param[out] timestamp   The timestamp of the data sample.
+ */
+void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+    data[0] = sensors.temp.calibrated[0];
+    if (timestamp)
+        *timestamp = sensors.temp.timestamp;
+    if (accuracy)
+        *accuracy = sensors.temp.accuracy;
+}
+
+/** Returns accuracy of gyro.
+ * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_gyro_accuracy(void)
+{
+    return sensors.gyro.accuracy;
+}
+
+/** Returns accuracy of compass.
+ * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_mag_accuracy(void)
+{
+    if (inv_data_builder.compass_disturbance)
+        return 0;
+    return sensors.compass.accuracy;
+}
+
+/** Returns accuracy of accel.
+ * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_accel_accuracy(void)
+{
+    return sensors.accel.accuracy;
+}
+
+inv_error_t inv_get_gyro_orient(int *orient)
+{
+    *orient = sensors.gyro.orientation;
+    return 0;
+}
+
+inv_error_t inv_get_accel_orient(int *orient)
+{
+    *orient = sensors.accel.orientation;
+    return 0;
+}
+
+
+/**
+ * @}
+ */
diff --git a/libsensors/software/core/mllite/data_builder.h b/libsensors/software/core/mllite/data_builder.h
new file mode 100644
index 0000000..b2d0881
--- /dev/null
+++ b/libsensors/software/core/mllite/data_builder.h
@@ -0,0 +1,224 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_DATA_BUILDER_H__
+#define INV_DATA_BUILDER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Uncomment this flag to enable playback debug and record or playback scenarios
+//#define INV_PLAYBACK_DBG
+
+/** This is a new sample of accel data */
+#define INV_ACCEL_NEW 1
+/** This is a new sample of gyro data */
+#define INV_GYRO_NEW 2
+/** This is a new sample of compass data */
+#define INV_MAG_NEW 4
+/** This is a new sample of temperature data */
+#define INV_TEMP_NEW 8
+/** This is a new sample of quaternion data */
+#define INV_QUAT_NEW 16
+
+/** Set if the data is contiguous. Typically not set if a sample was skipped */
+#define INV_CONTIGUOUS 16
+/** Set if the calibrated data has been solved for */
+#define INV_CALIBRATED 32
+/* INV_NEW_DATA set for a new set of data, cleared if not available. */
+#define INV_NEW_DATA 64
+/* Set if raw data exists */
+#define INV_RAW_DATA 128
+/* Set if the sensor is on */
+#define INV_SENSOR_ON 256
+/* Set if quaternion has bias correction applied */
+#define INV_BIAS_APPLIED 512
+
+#define INV_PRIORITY_MOTION_NO_MOTION          100
+#define INV_PRIORITY_GYRO_TC                   150
+#define INV_PRIORITY_QUATERNION_GYRO_ACCEL     200
+#define INV_PRIORITY_QUATERNION_NO_GYRO        250
+#define INV_PRIORITY_MAGNETIC_DISTURBANCE      300
+#define INV_PRIORITY_HEADING_FROM_GYRO         350
+#define INV_PRIORITY_COMPASS_BIAS_W_GYRO       375
+#define INV_PRIORITY_COMPASS_VECTOR_CAL        400
+#define INV_PRIORITY_COMPASS_ADV_BIAS          500
+#define INV_PRIORITY_9_AXIS_FUSION             600
+#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS  700
+#define INV_PRIORITY_QUATERNION_ACCURACY       750
+#define INV_PRIORITY_RESULTS_HOLDER            800
+#define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850
+#define INV_PRIORITY_HAL_OUTPUTS               900
+#define INV_PRIORITY_GLYPH                     950
+#define INV_PRIORITY_SM                        1000
+
+struct inv_single_sensor_t {
+    /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when
+    * the rotation matrix could be thought of only having elements of 0,1,-1.
+    * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign.
+    * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row.
+    * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row.
+    * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row.
+    */
+    int orientation;
+    /** The raw data in raw data units in the mounting frame */
+    short raw[3];
+    /** Calibrated data */
+    long calibrated[3];
+    long sensitivity;
+    /** Sample rate in microseconds */
+    long sample_rate_us;
+    long sample_rate_ms;
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */
+    int accuracy;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    /** Bandwidth in Hz */
+    int bandwidth;
+};
+struct inv_quat_sensor_t {
+    long raw[4];
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    inv_time_t timestamp;
+    long sample_rate_us;
+    long sample_rate_ms;
+};
+
+struct inv_sensor_cal_t {
+    struct inv_single_sensor_t gyro;
+    struct inv_single_sensor_t accel;
+    struct inv_single_sensor_t compass;
+    struct inv_single_sensor_t temp;
+    struct inv_quat_sensor_t quat;
+    /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate
+    * which data is a new sample as these data points may have different sample rates.
+    */
+    int status;
+};
+
+// Useful for debug record and playback
+typedef enum {
+    RD_NO_DEBUG,
+    RD_RECORD,
+    RD_PLAYBACK
+} rd_dbg_mode;
+
+typedef enum {
+    PLAYBACK_DBG_TYPE_GYRO,
+    PLAYBACK_DBG_TYPE_ACCEL,
+    PLAYBACK_DBG_TYPE_COMPASS,
+    PLAYBACK_DBG_TYPE_TEMPERATURE,
+    PLAYBACK_DBG_TYPE_EXECUTE,
+    PLAYBACK_DBG_TYPE_A_ORIENT,
+    PLAYBACK_DBG_TYPE_G_ORIENT,
+    PLAYBACK_DBG_TYPE_C_ORIENT,
+    PLAYBACK_DBG_TYPE_A_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_C_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_G_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_GYRO_OFF,
+    PLAYBACK_DBG_TYPE_ACCEL_OFF,
+    PLAYBACK_DBG_TYPE_COMPASS_OFF,
+    PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_QUAT
+
+} inv_rd_dbg_states;
+
+/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
+#define INV_MAX_DATA_CB 20
+
+#ifdef INV_PLAYBACK_DBG
+#include <stdio.h>
+void inv_turn_on_data_logging(FILE *file);
+void inv_turn_off_data_logging();
+#endif
+
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity);
+void inv_set_accel_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_compass_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_gyro_sample_rate(long sample_rate_us);
+void inv_set_compass_sample_rate(long sample_rate_us);
+void inv_set_quat_sample_rate(long sample_rate_us);
+void inv_set_accel_sample_rate(long sample_rate_us);
+void inv_set_gyro_bandwidth(int bandwidth_hz);
+void inv_set_accel_bandwidth(int bandwidth_hz);
+void inv_set_compass_bandwidth(int bandwidth_hz);
+
+inv_error_t inv_register_data_cb(inv_error_t (*func)
+                                 (struct inv_sensor_cal_t * data), int priority,
+                                 int sensor_type);
+inv_error_t inv_unregister_data_cb(inv_error_t (*func)
+                                   (struct inv_sensor_cal_t * data));
+
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp);
+inv_error_t inv_build_compass(const long *compass, int status,
+                                  inv_time_t timestamp);
+inv_error_t inv_build_accel(const long *accel, int status,
+                            inv_time_t timestamp);
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp);
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp);
+inv_error_t inv_execute_on_data(void);
+
+void inv_get_compass_bias(long *bias);
+
+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_bias_mask(const long *bias, int accuracy, int mask);
+
+void inv_get_gyro_bias(long *bias, long *temp);
+void inv_get_accel_bias(long *bias, long *temp);
+
+void inv_gyro_was_turned_off(void);
+void inv_accel_was_turned_off(void);
+void inv_compass_was_turned_off(void);
+void inv_quaternion_sensor_was_turned_off(void);
+inv_error_t inv_init_data_builder(void);
+long inv_get_gyro_sensitivity(void);
+long inv_get_accel_sensitivity(void);
+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_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+
+void inv_get_gyro(long *gyro);
+
+int inv_get_gyro_accuracy(void);
+int inv_get_accel_accuracy(void);
+int inv_get_mag_accuracy(void);
+
+int inv_get_compass_on(void);
+int inv_get_gyro_on(void);
+int inv_get_accel_on(void);
+
+inv_time_t inv_get_last_timestamp(void);
+int inv_get_compass_disturbance(void);
+
+//New DMP Cal Functions
+inv_error_t inv_get_gyro_orient(int *orient);
+inv_error_t inv_get_accel_orient(int *orient);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_DATA_BUILDER_H__ */
diff --git a/mlsdk/mllite/dmpconfig.txt b/libsensors/software/core/mllite/dmpconfig.txt
index 4643ed5..4643ed5 100644
--- a/mlsdk/mllite/dmpconfig.txt
+++ b/libsensors/software/core/mllite/dmpconfig.txt
diff --git a/libsensors/software/core/mllite/hal_outputs.c b/libsensors/software/core/mllite/hal_outputs.c
new file mode 100644
index 0000000..1cd3b81
--- /dev/null
+++ b/libsensors/software/core/mllite/hal_outputs.c
@@ -0,0 +1,425 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    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
+ *       @brief HAL Outputs.
+ */
+#include "hal_outputs.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+
+struct hal_output_t {
+    int accuracy_mag; /**< Compass accuracy */
+    int accuracy_gyro; /**< Gyro Accuracy */
+    int accuracy_accel;	/**< Accel Accuracy */
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    inv_time_t accel_timestamp;
+    long nav_quat[4];
+    int gyro_status;
+    int accel_status;
+    int compass_status;
+    int nine_axis_status;
+};
+
+static struct hal_output_t hal_out;
+
+/** Acceleration (m/s^2) in body frame.
+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
+*             should return a vector of magnitude near 9.81 m/s^2
+* @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_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp)
+{
+    int status;
+    /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
+     * So this 9.80665 / 2^16 */
+#define ACCEL_CONVERSION 0.000149637603759766f
+    long accel[3];
+    inv_get_accel_set(accel, accuracy, timestamp);
+    values[0] = accel[0] * ACCEL_CONVERSION;
+    values[1] = accel[1] * ACCEL_CONVERSION;
+    values[2] = accel[2] * ACCEL_CONVERSION;
+    if (hal_out.accel_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+/** Linear Acceleration (m/s^2) in Body Frame.
+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
+*             accel biases while at rest.
+* @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_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    long gravity[3], accel[3];
+
+    inv_get_accel_set(accel, accuracy, timestamp);
+    inv_get_gravity(gravity);
+    accel[0] -= gravity[0] >> 14;
+    accel[1] -= gravity[1] >> 14;
+    accel[2] -= gravity[2] >> 14;
+    values[0] = accel[0] * ACCEL_CONVERSION;
+    values[1] = accel[1] * ACCEL_CONVERSION;
+    values[2] = accel[2] * ACCEL_CONVERSION;
+
+    return hal_out.nine_axis_status;
+}
+
+/** Gravity vector (m/s^2) in Body Frame.
+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
+* @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_accel().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                 inv_time_t * timestamp)
+{
+    long gravity[3];
+    int status;
+
+    *accuracy = hal_out.accuracy_mag;
+    *timestamp = hal_out.nav_timestamp;
+    inv_get_gravity(gravity);
+    values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
+    values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
+    values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
+    if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA))
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+/** Gyroscope 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(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;
+
+    inv_get_gyro_set(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
+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
+* around an axis {x, y, z}. <br>
+* The three elements of the rotation vector are
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
+* equal to the direction of the axis of rotation.
+*
+* The three elements of the rotation vector are equal to the last three components of a unit quaternion
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
+*
+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
+* The reference coordinate system is defined as a direct orthonormal basis, where:
+
+    -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
+    -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
+    -Z points towards the sky and is perpendicular to the ground.
+* @param[out] values Length 4.
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+        inv_time_t * timestamp)
+{
+    *accuracy = hal_out.accuracy_mag;
+    *timestamp = hal_out.nav_timestamp;
+
+    if (hal_out.nav_quat[0] >= 0) {
+        values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
+        values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
+        values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
+        values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
+    } else {
+        values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
+        values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
+        values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
+        values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
+    }
+    values[4] = inv_get_heading_confidence_interval();
+
+    return hal_out.nine_axis_status;
+}
+
+
+/** Compass data (uT) in body frame.
+* @param[out] values Compass data in (uT), length 3. May be calibrated by having
+*             biases removed and sensitivity adjusted
+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
+* @param[out] timestamp Timestamp. In (ns) for Android.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+                                        inv_time_t * timestamp)
+{
+    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;
+    if (hal_out.compass_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
+
+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
+* @param[out] values Length 3, Degrees.<br>
+*        - values[0]: Azimuth, angle between the magnetic north direction
+*         and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
+*        - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
+*         when the z-axis moves toward the y-axis.<br>
+*        - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
+*          values when the x-axis moves toward the z-axis.<br>
+*
+* @note  This definition is different from yaw, pitch and roll used in aviation
+*        where the X axis is along the long side of the plane (tail to nose).
+*        Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
+*        in conjunction with remapCoordinateSystem() and getOrientation() to compute
+*        these values instead.
+*        Important note: For historical reasons the roll angle is positive in the
+*        clockwise direction (mathematically speaking, it should be positive in
+*        the counter-clockwise direction).
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor.
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+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;
+    *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];
+
+    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 
+* 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;
+    (void) sensor_cal;
+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag,
+                           &hal_out.nav_timestamp);
+    hal_out.gyro_status = sensor_cal->gyro.status;
+    hal_out.accel_status = sensor_cal->accel.status;
+    hal_out.compass_status = sensor_cal->compass.status;
+
+    // Find the highest sample rate and tie generating 9-axis to that one.
+    if (sensor_cal->gyro.status & INV_SENSOR_ON) {
+        sr = sensor_cal->gyro.sample_rate_ms;
+        use_sensor = 0;
+    }
+    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
+        sr = sensor_cal->accel.sample_rate_ms;
+        use_sensor = 1;
+    }
+    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
+        sr = sensor_cal->compass.sample_rate_ms;
+        use_sensor = 2;
+    }
+    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
+        sr = sensor_cal->quat.sample_rate_ms;
+        use_sensor = 3;
+    }
+
+    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; 
+        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; 
+        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; 
+        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; 
+        break;
+    }
+
+    return INV_SUCCESS;
+}
+
+/** Turns off generation of HAL outputs.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_stop_hal_outputs(void)
+{
+    inv_error_t result;
+    result = inv_unregister_data_cb(inv_generate_hal_outputs);
+    return result;
+}
+
+/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_start_hal_outputs(void)
+{
+    inv_error_t result;
+    result =
+        inv_register_data_cb(inv_generate_hal_outputs,
+                             INV_PRIORITY_HAL_OUTPUTS,
+                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
+    return result;
+}
+
+/** Initializes hal outputs class. This is called automatically by the
+* enable function. It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_hal_outputs(void)
+{
+    memset(&hal_out, 0, sizeof(hal_out));
+    return INV_SUCCESS;
+}
+
+/** Turns on creation and storage of HAL type results.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_enable_hal_outputs(void)
+{
+    inv_error_t result;
+    
+	// don't need to check the result for inv_init_hal_outputs 
+	// since it's always INV_SUCCESS
+	inv_init_hal_outputs();
+
+    result = inv_register_mpl_start_notification(inv_start_hal_outputs);
+    return result;
+}
+
+/** Turns off creation and storage of HAL type results.
+*/
+inv_error_t inv_disable_hal_outputs(void)
+{
+    inv_error_t result;
+
+    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/software/core/mllite/hal_outputs.h b/libsensors/software/core/mllite/hal_outputs.h
new file mode 100644
index 0000000..ed4cb65
--- /dev/null
+++ b/libsensors/software/core/mllite/hal_outputs.h
@@ -0,0 +1,43 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_HAL_OUTPUTS_H__
+#define INV_HAL_OUTPUTS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                           inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope(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,
+            inv_time_t * timestamp);
+
+    int inv_get_sensor_type_linear_acceleration(float *values,
+            int8_t *accuracy,
+            inv_time_t * timestamp);
+    int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp);
+
+    inv_error_t inv_enable_hal_outputs(void);
+    inv_error_t inv_disable_hal_outputs(void);
+    inv_error_t inv_init_hal_outputs(void);
+    inv_error_t inv_start_hal_outputs(void);
+    inv_error_t inv_stop_hal_outputs(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif                          // INV_HAL_OUTPUTS_H__
diff --git a/libsensors/software/core/mllite/invensense.h b/libsensors/software/core/mllite/invensense.h
new file mode 100644
index 0000000..fb8e12b
--- /dev/null
+++ b/libsensors/software/core/mllite/invensense.h
@@ -0,0 +1,28 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *  Main header file for Invensense's basic library.
+ */
+
+#include "data_builder.h"
+#include "hal_outputs.h"
+#include "message_layer.h"
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mpl.h"
+#include "results_holder.h"
+#include "start_manager.h"
+#include "storage_manager.h"
+#include "log.h"
+#include "mlinclude.h"
+//#include "..\HAL\include\mlos.h"
diff --git a/libsensors/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors/software/core/mllite/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..649b917
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/inv_sysfs_utils.c
@@ -0,0 +1,318 @@
+/**
+ *  @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/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors/software/core/mllite/linux/inv_sysfs_utils.h
new file mode 100644
index 0000000..45a35f9
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/inv_sysfs_utils.h
@@ -0,0 +1,84 @@
+/**
+ *  @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/software/core/mllite/linux/ml_load_dmp.c b/libsensors/software/core/mllite/linux/ml_load_dmp.c
new file mode 100644
index 0000000..f0f078c
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,281 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_LOAD_DMP
+ *
+ * @{
+ *      @file     ml_load_dmp.c
+ *      @brief    functions for writing dmp firmware.
+ */
+#include <stdio.h>
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-loaddmp"
+
+#include "ml_load_dmp.h"
+#include "log.h"
+#include "mlos.h"
+
+#define LOADDMP_LOG MPL_LOGI
+#define LOADDMP_LOG MPL_LOGI
+
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+#define DMP_CODE_SIZE 3060
+
+static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
+    /* 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, 
+    0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 
+    0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 
+    0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, 
+    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 
+    0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 
+    0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 
+    0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 
+    0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    /* bank # 1 */
+    0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 
+    0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 
+    0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 
+    0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 
+    0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 
+    0xff, 0x9b, 0xfc, 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, 0x10, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 
+    0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 
+    0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 
+    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, 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, 0x47, 0x78, 0xa2, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 
+    0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 
+    0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 
+    0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 3 */
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 
+    0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x40, 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, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+
+    /* bank # 4 */
+    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, 
+    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, 
+    0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 
+    0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 
+    0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 
+    0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 
+    0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 
+    0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, 
+    0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 
+    /* bank # 5 */
+    0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 
+    0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 
+    0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 
+    0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 
+    0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 
+    0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 
+    0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 
+    0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 
+    0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 
+    0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 
+    0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 
+    0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, 
+    0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, 
+    0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, 
+    0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, 
+    0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, 
+    /* bank # 6 */
+    0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, 
+    0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, 
+    0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, 
+    0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, 
+    0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 
+    0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, 
+    0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, 
+    0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, 
+    0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, 
+    0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, 
+    0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, 
+    0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, 
+    0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, 
+    0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, 
+    0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, 
+    0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, 
+    /* bank # 7 */
+    0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, 
+    0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, 
+    0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, 
+    0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, 
+    0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, 
+    0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, 
+    0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, 
+    0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, 
+    0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, 
+    0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, 
+    0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, 
+    0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, 
+    0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 
+    0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, 
+    0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, 
+    0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, 
+    /* bank # 8 */
+    0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, 
+    0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, 
+    0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, 
+    0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, 
+    0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, 
+    0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, 
+    0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0, 
+    0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, 
+    0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, 
+    0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, 
+    0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, 
+    0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, 
+    0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, 
+    0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, 
+    0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, 
+    0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, 
+    /* bank # 9 */
+    0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, 
+    0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, 
+    0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, 
+    0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, 
+    0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, 
+    0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, 
+    0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, 
+    0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, 
+    0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, 
+    0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, 
+    0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, 
+    0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, 
+    0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, 
+    0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, 
+    0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 
+    0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 
+    /* bank # 10 */
+    0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, 
+    0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 
+    0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 
+    0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 
+    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, 
+    /* 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
+};
+
+#define DMP_VERSION (dmpMemory)
+
+inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
+{
+    inv_error_t result = INV_SUCCESS;
+    int bytesWritten = 0;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("dmp firmware size to write = %d", len);
+    }
+    if ( fd == NULL ) {
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(dmp, 1, len, fd);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    return result;
+}
+
+inv_error_t inv_load_dmp(FILE *fd)
+{
+    inv_error_t result = INV_SUCCESS;
+    result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE);
+    return result;
+}
+
+/**
+ *  @}
+ */
diff --git a/libsensors/software/core/mllite/linux/ml_load_dmp.h b/libsensors/software/core/mllite/linux/ml_load_dmp.h
new file mode 100644
index 0000000..4d98692
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_load_dmp.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef INV_LOAD_DMP_H
+#define INV_LOAD_DMP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_dmp(FILE  *fd);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_LOAD_DMP_H */
diff --git a/libsensors/software/core/mllite/linux/ml_stored_data.c b/libsensors/software/core/mllite/linux/ml_stored_data.c
new file mode 100644
index 0000000..c5cf2e6
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_stored_data.c
@@ -0,0 +1,353 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_STORED_DATA
+ *
+ * @{
+ *      @file     ml_stored_data.c
+ *      @brief    functions for reading and writing stored data sets.
+ *                Typically, these functions process stored calibration data.
+ */
+
+#include <stdio.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
+#define STORECAL_DEBUG   0
+
+#define DEFAULT_KEY 29681
+
+#define STORECAL_LOG MPL_LOGI
+#define LOADCAL_LOG  MPL_LOGI
+
+inv_error_t inv_read_cal(unsigned char *cal, size_t len)
+{
+    FILE *fp;
+    int bytesRead;
+    inv_error_t result = INV_SUCCESS;
+
+    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);
+        result = INV_ERROR_FILE_READ;
+        goto read_cal_end;
+    }
+    else {
+        MPL_LOGI("Bytes read = %d", bytesRead);
+    }
+
+read_cal_end:
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_write_cal(unsigned char *cal, size_t len)
+{
+    FILE *fp;
+    int bytesWritten;
+    inv_error_t result = INV_SUCCESS;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("cal data size to write = %d", len);
+    }
+    fp = fopen(MLCAL_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(cal, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    fclose(fp);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 0 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature compensation : temperature data points,
+ *          - temperature compensation : gyro biases data points for X, Y,
+ *              and Z axes.
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data is produced internally by the MPL and its
+ *          size is 2777 bytes (header and checksum included).
+ *          Calibration format type 1 is currently used for ITG3500
+ *
+ *  @pre    inv_init_storage_manager()
+ *          must have been called.
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
+{
+    inv_error_t result;
+
+    LOADCAL_LOG("Entering inv_load_cal_V0\n");
+
+    /*if (len != expLen) {
+        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
+                 expLen, len);
+        return INV_ERROR_FILE_READ;
+    }*/
+
+    result = inv_load_mpl_states(calData, len);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 1 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature,
+ *          - gyro biases for X, Y, Z axes,
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data would normally be produced by the MPU Self
+ *          Test and its size is 36 bytes (header and checksum included).
+ *          Calibration format type 1 is produced by the MPU Self Test and
+ *          substitutes the type 0 : inv_load_cal_V0().
+ *
+ *  @pre    
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
+{
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   Loads a set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *
+ * @pre     
+ *          
+ *
+ * @param   calData
+ *              A pointer to an array of bytes to be parsed.
+ *
+ * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal(unsigned char *calData)
+{
+    int calType = 0;
+    int len = 0;
+    //int ptr;
+    //uint32_t chk = 0;
+    //uint32_t cmp_chk = 0;
+
+    /*load_func_t loaders[] = {
+        inv_load_cal_V0,
+        inv_load_cal_V1,
+    };
+    */
+
+    inv_load_cal_V0(calData, len);
+
+    /* read the header (type and len)
+       len is the total record length including header and checksum */
+    len = 0;
+    len += 16777216L * ((int)calData[0]);
+    len += 65536L * ((int)calData[1]);
+    len += 256 * ((int)calData[2]);
+    len += (int)calData[3];
+
+    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
+    if (calType > 5) {
+        MPL_LOGE("Unsupported calibration file format %d. "
+                 "Valid types 0..5\n", calType);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    /* call the proper method to read in the data */
+    //return loaders[calType] (calData, len);
+    return 0;
+}
+
+/**
+ *  @brief  Stores a set of calibration data.
+ *          It generates a binary data set containing calibration data.
+ *          The binary data set is intended to be stored into a file.
+ *
+ *  @pre    inv_dmp_open()
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be stored.
+ *  @param  length
+ *              The amount of bytes available in the array.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_store_cal(unsigned char *calData, size_t length)
+{
+    inv_error_t res = 0;
+    size_t size;
+
+    STORECAL_LOG("Entering inv_store_cal\n");
+
+    inv_get_mpl_state_size(&size);
+
+    MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
+
+    /* store data */
+    res = inv_save_mpl_states(calData, size);
+    if(res != 0)
+    {
+        MPL_LOGE("inv_save_mpl_states() failed");
+    }
+
+    STORECAL_LOG("Exiting inv_store_cal\n");
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Load a calibration file.
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_load_calibration(void)
+{
+    unsigned char *calData;
+    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;
+    }
+
+    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);
+    if(result != INV_SUCCESS) {
+        MPL_LOGE("Could not load cal file - "
+                 "aborting\n");
+    }
+
+    result = inv_load_mpl_states(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not load the calibration data - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Store runtime calibration data to a file
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_store_calibration(void)
+{
+    unsigned char *calData;
+    inv_error_t result;
+    size_t length;
+
+    result = inv_get_mpl_state_size(&length);
+    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;
+    }
+    else {
+        MPL_LOGI("mpl state size = %d", length);
+    }
+
+    result = inv_save_mpl_states(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not save mpl states - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+    else {
+        MPL_LOGE("calData from inv_save_mpl_states, size=%d", 
+                 strlen((char *)calData));
+    }
+
+    result = inv_write_cal(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not store calibrated data on file - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return INV_SUCCESS;
+}
+
+/**
+ *  @}
+ */
diff --git a/libsensors/software/core/mllite/linux/ml_stored_data.h b/libsensors/software/core/mllite/linux/ml_stored_data.h
new file mode 100644
index 0000000..336f081
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_stored_data.h
@@ -0,0 +1,53 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $
+ *
+ ******************************************************************************/
+
+#ifndef INV_MPL_STORED_DATA_H
+#define INV_MPL_STORED_DATA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    Defines
+*/
+#define MLCAL_FILE "/data/inv_cal_data.bin"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_calibration(void);
+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_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);
+
+/*
+    Other prototypes
+*/
+inv_error_t inv_load_cal(unsigned char *calData);
+inv_error_t inv_store_cal(unsigned char *calData, size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_MPL_STORED_DATA_H */
diff --git a/libsensors/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100644
index 0000000..5636a02
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,416 @@
+#include <string.h>
+#include <stdio.h>
+#include "ml_sysfs_helper.h"
+#include <dirent.h>
+#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,
+	CMD_GET_CHIP_NAME,
+	CMD_GET_SYSFS_KEY,
+	CMD_GET_TRIGGER_PATH,
+	CMD_GET_DEVICE_NODE
+};
+static char sysfs_path[100];
+static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"};
+static int chip_ind;
+static int initialized =0;
+static int status = 0;
+static int iio_initialized = 0;
+static int iio_dev_num = 0;
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+static const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * find_type_by_name() - function to match top level types by name
+ * @name: top level type instance name
+ * @type: the type of top level instance being sort
+ *
+ * Typical types this is used for are device and trigger.
+ **/
+int find_type_by_name(const char *name, const char *type)
+{
+	const struct dirent *ent;
+	int number, numstrlen;
+
+	FILE *nameFile;
+	DIR *dp;
+	char thisname[IIO_MAX_NAME_LENGTH];
+	char *filename;
+
+	dp = opendir(iio_dir);
+	if (dp == NULL) {
+		printf("No industrialio devices available");
+		return -ENODEV;
+	}
+
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name, ".") != 0 &&
+			strcmp(ent->d_name, "..") != 0 &&
+			strlen(ent->d_name) > strlen(type) &&
+			strncmp(ent->d_name, type, strlen(type)) == 0) {
+			numstrlen = sscanf(ent->d_name + strlen(type),
+					   "%d",
+					   &number);
+			/* verify the next character is not a colon */
+			if (strncmp(ent->d_name + strlen(type) + numstrlen,
+					":",
+					1) != 0) {
+				filename = malloc(strlen(iio_dir)
+						+ strlen(type)
+						+ numstrlen
+						+ 6);
+				if (filename == NULL)
+					return -ENOMEM;
+				sprintf(filename, "%s%s%d/name",
+					iio_dir,
+					type,
+					number);
+				nameFile = fopen(filename, "r");
+				if (!nameFile)
+					continue;
+				free(filename);
+				fscanf(nameFile, "%s", thisname);
+				if (strcmp(name, thisname) == 0)
+					return number;
+				fclose(nameFile);
+			}
+		}
+	}
+	return -ENODEV;
+}
+
+/* mode 0: search for which chip in the system and fill sysfs path
+   mode 1: return event number
+ */
+static int parsing_proc_input(int mode, char *name){
+	const char input[] = "/proc/bus/input/devices";
+	char line[100], d;
+	char tmp[100];
+	FILE *fp;
+	int i, j, result, find_flag;
+	int event_number = -1;
+	int input_number = -1;
+
+	if(NULL == (fp = fopen(input, "rt")) ){
+		return -1;
+	}
+	result = 1;
+	find_flag = 0;
+	while(result != 0 && find_flag < 2){
+		i = 0;
+		d = 0;
+		memset(line, 0, 100);
+		while(d != '\n'){		
+			result = fread(&d, 1, 1, fp);
+			if(result == 0){
+				line[0] = 0;
+				break;
+			}
+			sprintf(&line[i], "%c", d);		
+			i ++;
+		}
+		if(line[0] == 'N'){
+			i = 1;
+			while(line[i] != '"'){
+				i++;
+			}
+			i++;
+			j = 0;
+			find_flag = 0;
+			if (mode == 0){
+				while(j < CHIP_NUM){
+					if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){
+						find_flag = 1;
+						chip_ind = j;
+					}
+					j++;
+				}
+			} else if (mode  != 0){
+				if(!memcmp(&line[i], name, strlen(name))){
+					find_flag = 1;
+				}
+			}
+		}		
+		if(find_flag){
+			if(mode == 0){
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}	
+					sprintf(sysfs_path, "%s%s", "/sys", tmp);
+					find_flag++;
+				}
+			} else if(mode == 1){
+				if(line[0] == 'H') {
+					i = 2;
+					while(line[i] != '=') i++;
+					while(line[i] != 't') i++;	
+					i++;
+					event_number = 0;
+					while(line[i] != '\n'){
+						if(line[i] >= '0' && line[i] <= '9')
+							event_number = event_number*10 + line[i]-0x30;
+						i ++;
+					}
+					find_flag ++;
+				}
+			} else if (mode == 2) {
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}
+					input_number = 0;
+					if(tmp[j-2] >= '0' && tmp[j-2] <= '9') 
+						input_number += (tmp[j-2]-0x30)*10;
+					if(tmp[j-1] >= '0' && tmp[j-1] <= '9') 
+						input_number += (tmp[j-1]-0x30);
+					find_flag++;
+				}
+			}
+		}
+	}
+	fclose(fp);
+	if(find_flag == 0){
+		return -1;
+	}
+	if(0 == mode)
+		status = 1;
+	if (mode == 1)
+		return event_number;
+	if (mode == 2)
+		return input_number;
+	return 0;
+
+}
+static void init_iio() {
+	int i, j;
+	char iio_chip[10];
+	int dev_num;
+	for(j=0; j< CHIP_NUM; j++) {
+		for (i=0; i<strlen(chip_name[j]); i++) {
+			iio_chip[i] = tolower(chip_name[j][i]);
+		}
+		iio_chip[strlen(chip_name[0])] = '\0';
+		dev_num = find_type_by_name(iio_chip, "iio:device");
+		if(dev_num >= 0) {
+			iio_initialized = 1;
+			iio_dev_num = dev_num;
+			chip_ind = j;
+		}
+	}
+}
+
+static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data)
+{
+	char key_path[100];
+	FILE *fp;
+	int i, result;
+	if(initialized == 0){
+		parsing_proc_input(0, NULL);
+		initialized = 1;
+	}
+	if(initialized && status == 0) {
+		init_iio();
+		if (iio_initialized == 0)
+			return -1;
+	}
+
+	memset(key_path, 0, 100);
+	switch(cmd){
+	case CMD_GET_SYSFS_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu");
+		break;
+	case CMD_GET_DMP_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware");
+		break;
+	case CMD_GET_CHIP_NAME:
+		sprintf(data, "%s", chip_name[chip_ind]);
+		break;
+	case CMD_GET_TRIGGER_PATH:
+		sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num);
+		break;
+	case CMD_GET_DEVICE_NODE:
+		sprintf(data, "/dev/iio:device%d", iio_dev_num);
+		break;
+	case CMD_GET_SYSFS_KEY:
+		memset(key_path, 0, 100);
+		if (iio_initialized == 1)
+			sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num);
+		else	
+			sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key");
+
+		if((fp = fopen(key_path, "rt")) == NULL)
+			return -1;
+		for(i=0;i<16;i++){
+			fscanf(fp, "%02x", &result);
+			data[i] = (char)result;
+		}
+		
+		fclose(fp);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+/** 
+ *  @brief  return sysfs key. if the key is not available
+ *          return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the key
+ *           It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_key(unsigned char *key)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the sysfs path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the sysfs
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_path(char *name)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+inv_error_t inv_get_sysfs_abs_path(char *name)
+{
+    strcpy(name, MPU_SYSFS_ABS_PATH);
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the dmp file path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the dmp file
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_dmpfile(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return the chip name. If the chip is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_chip_name(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return event handler number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: event number store
+ */
+inv_error_t  inv_get_handler_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(1, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;	
+}
+
+/** 
+ *  @brief  return input number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: input number store
+ */
+inv_error_t  inv_get_input_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(2, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else {
+		return INV_SUCCESS;
+	}	
+}
+
+/** 
+ *  @brief  return iio trigger name. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the trigger
+ *           name. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_trigger_path(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return iio device node. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the device
+ *           node. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_device_node(const char *name)
+{
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
diff --git a/libsensors/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100644
index 0000000..eb285c5
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/ml_sysfs_helper.h
@@ -0,0 +1,36 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_SYSFS_HELPER_H__
+#define MLDMP_SYSFS_HELPER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "invensense.h"
+
+int find_type_by_name(const char *name, const char *type);
+inv_error_t inv_get_sysfs_path(char *name);
+inv_error_t inv_get_sysfs_abs_path(char *name);
+inv_error_t inv_get_dmpfile(char *name);
+inv_error_t inv_get_chip_name(char *name);
+inv_error_t inv_get_sysfs_key(unsigned char *key);
+inv_error_t inv_get_handler_number(const char *name, int *num);
+inv_error_t inv_get_input_number(const char *name, int *num);
+inv_error_t inv_get_iio_trigger_path(const char *name);
+inv_error_t inv_get_iio_device_node(const char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/mlsdk/platform/include/mlos.h b/libsensors/software/core/mllite/linux/mlos.h
index 78f0a83..287025f 100644
--- a/mlsdk/platform/include/mlos.h
+++ b/libsensors/software/core/mllite/linux/mlos.h
@@ -1,19 +1,7 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 
 #ifndef _MLOS_H
@@ -77,11 +65,13 @@ typedef unsigned int HANDLE;
 	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;
diff --git a/libsensors/software/core/mllite/linux/mlos_linux.c b/libsensors/software/core/mllite/linux/mlos_linux.c
new file mode 100644
index 0000000..75f062e
--- /dev/null
+++ b/libsensors/software/core/mllite/linux/mlos_linux.c
@@ -0,0 +1,194 @@
+/*
+ $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/software/core/mllite/message_layer.c b/libsensors/software/core/mllite/message_layer.c
new file mode 100644
index 0000000..8317957
--- /dev/null
+++ b/libsensors/software/core/mllite/message_layer.c
@@ -0,0 +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;

+}

+

+/**

+ * @}

+ */

diff --git a/libsensors/software/core/mllite/message_layer.h b/libsensors/software/core/mllite/message_layer.h
new file mode 100644
index 0000000..df0c0e2
--- /dev/null
+++ b/libsensors/software/core/mllite/message_layer.h
@@ -0,0 +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__

diff --git a/libsensors/software/core/mllite/ml_math_func.c b/libsensors/software/core/mllite/ml_math_func.c
new file mode 100644
index 0000000..86c4b41
--- /dev/null
+++ b/libsensors/software/core/mllite/ml_math_func.c
@@ -0,0 +1,660 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *   @defgroup  ML_MATH_FUNC ml_math_func
+ *   @brief     Motion Library - Math Functions
+ *              Common math functions the Motion Library
+ *
+ *   @{
+ *       @file ml_math_func.c
+ *       @brief Math Functions.
+ */
+
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mlinclude.h"
+#include <string.h>
+
+/** @internal
+ * Does the cross product of compass by gravity, then converts that
+ * to the world frame using the quaternion, then computes the angle that
+ * is made.
+ *
+ * @param[in] compass Compass Vector (Body Frame), length 3
+ * @param[in] grav Gravity Vector (Body Frame), length 3
+ * @param[in] quat Quaternion, Length 4
+ * @return Angle Cross Product makes after quaternion rotation.
+ */
+float inv_compass_angle(const long *compass, const long *grav, const float *quat)
+{
+    float cgcross[4], q1[4], q2[4], qi[4];
+    float angW;
+
+    // Compass cross Gravity
+    cgcross[0] = 0.f;
+    cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+
+    // Now convert cross product into world frame
+    inv_q_multf(quat, cgcross, q1);
+    inv_q_invertf(quat, qi);
+    inv_q_multf(q1, qi, q2);
+
+    // Protect against atan2 of 0,0
+    if ((q2[2] == 0.f) && (q2[1] == 0.f))
+        return 0.f;
+
+    // This is the unfiltered heading correction
+    angW = -atan2f(q2[2], q2[1]);
+    return angW;
+}
+
+/**
+ *  @brief  The gyro data magnitude squared :
+ *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
+ * @param[in] gyro Gyro data scaled with 1 dps = 2^16
+ *  @return the computed magnitude squared output of the gyroscope.
+ */
+unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
+{
+    unsigned long gmag = 0;
+    long temp;
+    int kk;
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
+        gmag += temp * temp;
+    }
+
+    return gmag;
+}
+
+/** Performs a multiply and shift by 29. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>29
+*/
+long inv_q29_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 29));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 29);
+    return result;
+#endif
+}
+
+/** Performs a multiply and shift by 30. These are good functions to write in assembly on
+ * with devices with small memory where you want to get rid of the long long which some
+ * assemblers don't handle well
+ * @param[in] a
+ * @param[in] b
+ * @return ((long long)a*b)>>30
+*/
+long inv_q30_mult(long a, long b)
+{
+#ifdef UMPL_ELIMINATE_64BIT
+    long result;
+    result = (long)((float)a * b / (1L << 30));
+    return result;
+#else
+    long long temp;
+    long result;
+    temp = (long long)a * b;
+    result = (long)(temp >> 30);
+    return result;
+#endif
+}
+
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q30_div(long a, long b)
+{
+    long long temp;
+    long result;
+    temp = (((long long)a) << 30) / b;
+    result = (long)temp;
+    return result;
+}
+#endif
+
+/** Performs a multiply and shift by shift. These are good functions to write
+ * in assembly on with devices with small memory where you want to get rid of
+ * the long long which some assemblers don't handle well
+ * @param[in] a First multicand
+ * @param[in] b Second multicand
+ * @param[in] shift Shift amount after multiplying
+ * @return ((long long)a*b)<<shift
+*/
+#ifndef UMPL_ELIMINATE_64BIT
+long inv_q_shift_mult(long a, long b, int shift)
+{
+    long result;
+    result = (long)(((long long)a * b) >> shift);
+    return result;
+}
+#endif
+
+/** Performs a fixed point quaternion multiply.
+* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qProd Product after quaternion multiply. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_mult(const long *q1, const long *q2, long *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
+               inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);
+
+    qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
+               inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);
+
+    qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
+               inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);
+
+    qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
+               inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
+}
+
+/** Performs a fixed point quaternion addition.
+* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
+*            to 2^30
+* @param[out] qSum Sum after quaternion summation. Length 4.
+*             1.0 scaled to 2^30.
+*/
+void inv_q_add(long *q1, long *q2, long *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_vector_normalize(long *vec, int length)
+{
+    INVENSENSE_FUNC_START;
+    double normSF = 0;
+    int ii;
+    for (ii = 0; ii < length; ii++) {
+        normSF +=
+            inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
+    }
+    if (normSF > 0) {
+        normSF = 1 / sqrt(normSF);
+        for (ii = 0; ii < length; ii++) {
+            vec[ii] = (int)((double)vec[ii] * normSF);
+        }
+    } else {
+        vec[0] = 1073741824L;
+        for (ii = 1; ii < length; ii++) {
+            vec[ii] = 0;
+        }
+    }
+}
+
+void inv_q_normalize(long *q)
+{
+    INVENSENSE_FUNC_START;
+    inv_vector_normalize(q, 4);
+}
+
+void inv_q_invert(const long *q, long *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+double quaternion_to_rotation_angle(const long *quat) {
+    double quat0 = (double )quat[0] / 1073741824;
+    if (quat0 > 1.0f) {
+        quat0 = 1.0;
+    } else if (quat0 < -1.0f) {
+        quat0 = -1.0;
+    }
+
+    return acos(quat0)*2*180/M_PI;
+}
+
+/** Rotates a 3-element vector by Rotation defined by Q
+*/
+void inv_q_rotate(const long *q, const long *in, long *out)
+{
+    long q_temp1[4], q_temp2[4];
+    long in4[4], out4[4];
+
+    // Fixme optimize
+    in4[0] = 0;
+    memcpy(&in4[1], in, 3 * sizeof(long));
+    inv_q_mult(q, in4, q_temp1);
+    inv_q_invert(q, q_temp2);
+    inv_q_mult(q_temp1, q_temp2, out4);
+    memcpy(out, &out4[1], 3 * sizeof(long));
+}
+
+void inv_q_multf(const float *q1, const float *q2, float *qProd)
+{
+    INVENSENSE_FUNC_START;
+    qProd[0] =
+        (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
+    qProd[1] =
+        (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
+    qProd[2] =
+        (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
+    qProd[3] =
+        (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
+}
+
+void inv_q_addf(const float *q1, const float *q2, float *qSum)
+{
+    INVENSENSE_FUNC_START;
+    qSum[0] = q1[0] + q2[0];
+    qSum[1] = q1[1] + q2[1];
+    qSum[2] = q1[2] + q2[2];
+    qSum[3] = q1[3] + q2[3];
+}
+
+void inv_q_normalizef(float *q)
+{
+    INVENSENSE_FUNC_START;
+    float normSF = 0;
+    float xHalf = 0;
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (normSF < 2) {
+        xHalf = 0.5f * normSF;
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        normSF = normSF * (1.5f - xHalf * normSF * normSF);
+        q[0] *= normSF;
+        q[1] *= normSF;
+        q[2] *= normSF;
+        q[3] *= normSF;
+    } else {
+        q[0] = 1.0;
+        q[1] = 0.0;
+        q[2] = 0.0;
+        q[3] = 0.0;
+    }
+    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+}
+
+/** Performs a length 4 vector normalization with a square root.
+* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
+*/
+void inv_q_norm4(float *q)
+{
+    float mag;
+    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    if (mag) {
+        q[0] /= mag;
+        q[1] /= mag;
+        q[2] /= mag;
+        q[3] /= mag;
+    } else {
+        q[0] = 1.f;
+        q[1] = 0.f;
+        q[2] = 0.f;
+        q[3] = 0.f;
+    }
+}
+
+void inv_q_invertf(const float *q, float *qInverted)
+{
+    INVENSENSE_FUNC_START;
+    qInverted[0] = q[0];
+    qInverted[1] = -q[1];
+    qInverted[2] = -q[2];
+    qInverted[3] = -q[3];
+}
+
+/**
+ * Converts a quaternion to a rotation matrix.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
+ *             First 3 elements of the rotation matrix, represent
+ *             the first row of the matrix. Rotation matrix multiplied
+ *             by a 3 element column vector transform a vector from Body
+ *             to World.
+ */
+void inv_quaternion_to_rotation(const long *quat, long *rot)
+{
+    rot[0] =
+        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[1] =
+        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
+    rot[2] =
+        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
+    rot[3] =
+        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
+    rot[4] =
+        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+    rot[5] =
+        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
+    rot[6] =
+        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
+    rot[7] =
+        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
+    rot[8] =
+        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
+                quat[0]) -
+        1073741824L;
+}
+
+/**
+ * Converts a quaternion to a rotation vector. A rotation vector is
+ * a method to represent a 4-element quaternion vector in 3-elements.
+ * To get the quaternion from the 3-elements, The last 3-elements of
+ * the quaternion will be the given rotation vector. The first element
+ * of the quaternion will be the positive value that will be required
+ * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
+ * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
+ * @param[out] rot Rotation vector in fixed point. One is 2^30.
+ */
+void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
+{
+    rot[0] = quat[1];
+    rot[1] = quat[2];
+    rot[2] = quat[3];
+
+    if (quat[0] < 0.0) {
+        rot[0] = -rot[0];
+        rot[1] = -rot[1];
+        rot[2] = -rot[2];
+    }
+}
+
+/** Converts a 32-bit long to a big endian byte stream */
+unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 24) & 0xff);
+    big8[1] = (unsigned char)((x >> 16) & 0xff);
+    big8[2] = (unsigned char)((x >> 8) & 0xff);
+    big8[3] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+/** Converts a big endian byte stream into a 32-bit long */
+long inv_big8_to_int32(const unsigned char *big8)
+{
+    long x;
+    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
+        | ((long)big8[3]);
+    return x;
+}
+
+/** Converts a big endian byte stream into a 16-bit integer (short) */
+short inv_big8_to_int16(const unsigned char *big8)
+{
+    short x;
+    x = ((short)big8[0] << 8) | ((short)big8[1]);
+    return x;
+}
+
+/** Converts a little endian byte stream into a 16-bit integer (short) */
+short inv_little8_to_int16(const unsigned char *little8)
+{
+    short x;
+    x = ((short)little8[1] << 8) | ((short)little8[0]);
+    return x;
+}
+
+/** Converts a 16-bit short to a big endian byte stream */
+unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
+{
+    big8[0] = (unsigned char)((x >> 8) & 0xff);
+    big8[1] = (unsigned char)(x & 0xff);
+    return big8;
+}
+
+void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
+{
+    int k, l, i, j;
+    for (i = 0, k = 0; i < *n; i++, k++) {
+        for (j = 0, l = 0; j < *n; j++, l++) {
+            if (i == x)
+                i++;
+            if (j == y)
+                j++;
+            *(b + 6 * k + l) = *(a + 6 * i + j);
+        }
+    }
+    *n = *n - 1;
+}
+
+float inv_matrix_det(float *p, int *n)
+{
+    float d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_inc(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_det(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+double inv_matrix_detd(double *p, int *n)
+{
+    double d[6][6], sum = 0;
+    int i, j, m;
+    m = *n;
+    if (*n == 2)
+        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
+    for (i = 0, j = 0; j < m; j++) {
+        *n = m;
+        inv_matrix_det_incd(p, &d[0][0], n, i, j);
+        sum =
+            sum + *(p + 6 * i + j) * SIGNM(i +
+                                            j) *
+            inv_matrix_detd(&d[0][0], n);
+    }
+
+    return (sum);
+}
+
+/** Wraps angle from (-M_PI,M_PI]
+ * @param[in] ang Angle in radians to wrap
+ * @return Wrapped angle from (-M_PI,M_PI]
+ */
+float inv_wrap_angle(float ang)
+{
+    if (ang > M_PI)
+        return ang - 2 * (float)M_PI;
+    else if (ang <= -(float)M_PI)
+        return ang + 2 * (float)M_PI;
+    else
+        return ang;
+}
+
+/** Finds the minimum angle difference ang1-ang2 such that difference
+ * is between [-M_PI,M_PI]
+ * @param[in] ang1
+ * @param[in] ang2
+ * @return angle difference ang1-ang2
+ */
+float inv_angle_diff(float ang1, float ang2)
+{
+    float d;
+    ang1 = inv_wrap_angle(ang1);
+    ang2 = inv_wrap_angle(ang2);
+    d = ang1 - ang2;
+    if (d > M_PI)
+        d -= 2 * (float)M_PI;
+    else if (d < -(float)M_PI)
+        d += 2 * (float)M_PI;
+    return d;
+}
+
+/** bernstein hash, derived from public domain source */
+uint32_t inv_checksum(const unsigned char *str, int len)
+{
+    uint32_t hash = 5381;
+    int i, c;
+
+    for (i = 0; i < len; i++) {
+        c = *(str + i);
+        hash = ((hash << 5) + hash) + c;	/* hash * 33 + c */
+    }
+
+    return hash;
+}
+
+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;
+}
+
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor be:
+* 010_001_000 or 0x88 in hex.
+*/
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
+{
+
+    unsigned short scalar;
+
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+
+    return scalar;
+}
+
+/** Uses the scalar orientation value to convert from chip frame to body frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
+{
+    output[0] = input[orientation      & 0x03] * SIGNSET(orientation & 0x004);
+    output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
+    output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
+}
+
+/** Uses the scalar orientation value to convert from body frame to chip frame
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
+{
+    output[orientation & 0x03]      = input[0] * SIGNSET(orientation & 0x004);
+    output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
+    output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
+}
+
+
+/** Uses the scalar orientation value to convert from chip frame to body frame and
+* apply appropriate scaling.
+* @param[in] orientation A scalar that represent how to go from chip to body frame
+* @param[in] sensitivity Sensitivity scale
+* @param[in] input Input vector, length 3
+* @param[out] output Output vector, length 3
+*/
+void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output)
+{
+    output[0] = inv_q30_mult(input[orientation & 0x03] *
+                             SIGNSET(orientation & 0x004), sensitivity);
+    output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
+                             SIGNSET(orientation & 0x020), sensitivity);
+    output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
+                             SIGNSET(orientation & 0x100), sensitivity);
+}
+
+/** find a norm for a vector
+* @param[in] a vector [3x1]
+* @param[out] output the norm of the input vector
+*/
+double inv_vector_norm(const float *x)
+{
+    return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
+}
+/**
+ * @}
+ */
diff --git a/libsensors/software/core/mllite/ml_math_func.h b/libsensors/software/core/mllite/ml_math_func.h
new file mode 100644
index 0000000..916de0a
--- /dev/null
+++ b/libsensors/software/core/mllite/ml_math_func.h
@@ -0,0 +1,108 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INVENSENSE_INV_MATH_FUNC_H__
+#define INVENSENSE_INV_MATH_FUNC_H__
+
+#include "mltypes.h"
+
+#define GYRO_MAG_SQR_SHIFT 6
+#define NUM_ROTATION_MATRIX_ELEMENTS (9)
+#define ROT_MATRIX_SCALE_LONG  (1073741824L)
+#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f)
+#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \
+    ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT ))
+#define SIGNM(k)((int)(k)&1?-1:1)
+#define SIGNSET(x) ((x) ? -1 : +1)
+
+#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    static inline float inv_q30_to_float(long q30)
+    {
+        return (float) q30 / ((float)(1L << 30));
+    }
+
+    static inline double inv_q30_to_double(long q30)
+    {
+        return (double) q30 / ((double)(1L << 30));
+    }
+
+    static inline float inv_q16_to_float(long q16)
+    {
+        return (float) q16 / (1L << 16);
+    }
+
+    static inline double inv_q16_to_double(long q16)
+    {
+        return (double) q16 / (1L << 16);
+    }
+
+
+
+
+    long inv_q29_mult(long a, long b);
+    long inv_q30_mult(long a, long b);
+
+    /* UMPL_ELIMINATE_64BIT Notes:
+     * An alternate implementation using float instead of long long accudoublemulators
+     * is provided for q29_mult and q30_mult.
+     * When long long accumulators are used and an alternate implementation is not
+     * available, we eliminate the entire function and header with a macro.
+     */
+#ifndef UMPL_ELIMINATE_64BIT
+    long inv_q30_div(long a, long b);
+    long inv_q_shift_mult(long a, long b, int shift);
+#endif
+
+    void inv_q_mult(const long *q1, const long *q2, long *qProd);
+    void inv_q_add(long *q1, long *q2, long *qSum);
+    void inv_q_normalize(long *q);
+    void inv_q_invert(const long *q, long *qInverted);
+    void inv_q_multf(const float *q1, const float *q2, float *qProd);
+    void inv_q_addf(const float *q1, const float *q2, float *qSum);
+    void inv_q_normalizef(float *q);
+    void inv_q_norm4(float *q);
+    void inv_q_invertf(const float *q, float *qInverted);
+    void inv_quaternion_to_rotation(const long *quat, long *rot);
+    unsigned char *inv_int32_to_big8(long x, unsigned char *big8);
+    long inv_big8_to_int32(const unsigned char *big8);
+    short inv_big8_to_int16(const unsigned char *big8);
+    short inv_little8_to_int16(const unsigned char *little8);
+    unsigned char *inv_int16_to_big8(short x, unsigned char *big8);
+    float inv_matrix_det(float *p, int *n);
+    void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y);
+    double inv_matrix_detd(double *p, int *n);
+    void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y);
+    float inv_wrap_angle(float ang);
+    float inv_angle_diff(float ang1, float ang2);
+    void inv_quaternion_to_rotation_vector(const long *quat, long *rot);
+    unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
+    void inv_convert_to_body(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_chip(unsigned short orientation, const long *input, long *output);
+    void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
+    void inv_q_rotate(const long *q, const long *in, long *out);
+	void inv_vector_normalize(long *vec, int length);
+    uint32_t inv_checksum(const unsigned char *str, int len);
+    float inv_compass_angle(const long *compass, const long *grav,
+                            const float *quat);
+    unsigned long inv_get_gyro_sum_of_sqr(const long *gyro);
+
+    static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2)
+    {
+        return (long)((t1 - t2) / 1000000L);
+    }
+
+    double quaternion_to_rotation_angle(const long *quat);
+    double inv_vector_norm(const float *x);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/libsensors/software/core/mllite/mlmath.c b/libsensors/software/core/mllite/mlmath.c
new file mode 100644
index 0000000..5eb4264
--- /dev/null
+++ b/libsensors/software/core/mllite/mlmath.c
@@ -0,0 +1,68 @@
+/*
+ $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/software/core/mllite/mpl.c b/libsensors/software/core/mllite/mpl.c
new file mode 100644
index 0000000..231cbfd
--- /dev/null
+++ b/libsensors/software/core/mllite/mpl.c
@@ -0,0 +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;

+}

+

+/**

+ * @}

+ */

diff --git a/libsensors/software/core/mllite/mpl.h b/libsensors/software/core/mllite/mpl.h
new file mode 100644
index 0000000..a6b5ac7
--- /dev/null
+++ b/libsensors/software/core/mllite/mpl.h
@@ -0,0 +1,24 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_MPL_H__
+#define INV_MPL_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_init_mpl(void);
+inv_error_t inv_start_mpl(void);
+inv_error_t inv_get_version(char **version);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MPL_H__
diff --git a/libsensors/software/core/mllite/results_holder.c b/libsensors/software/core/mllite/results_holder.c
new file mode 100644
index 0000000..97ffdec
--- /dev/null
+++ b/libsensors/software/core/mllite/results_holder.c
@@ -0,0 +1,500 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  Results_Holder results_holder
+ *   @brief     Motion Library - Results Holder
+ *              Holds the data for MPL
+ *
+ *   @{
+ *       @file results_holder.c
+ *       @brief Results Holder for HAL.
+ */
+#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"
+
+// These 2 status bits are used to control when the 9 axis quaternion is updated
+#define INV_COMPASS_CORRECTION_SET 1
+#define INV_6_AXIS_QUAT_SET 2
+
+struct results_t {
+    long nav_quat[4];
+    long gam_quat[4];
+    inv_time_t nav_timestamp;
+    inv_time_t gam_timestamp;
+    long local_field[3]; /**< local earth's magnetic field */
+    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 */
+    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 */
+    long compass_count; /**< compass state internal counter */
+    int got_compass_bias; /**< Flag describing if compass bias is known */
+    int large_mag_field; /**< Flag describing if there is a large magnetic field */
+    int compass_state; /**< Internal compass state */
+    long status;
+    struct inv_sensor_cal_t *sensor;
+    float quat_confidence_interval;
+};
+static struct results_t rh;
+
+/** @internal
+* Store a quaternion more suitable for gaming. This quaternion is often determined
+* using only gyro and accel.
+* @param[in] quat Length 4, Quaternion scaled by 2^30
+*/
+void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp)
+{
+    rh.status |= INV_6_AXIS_QUAT_SET;
+    memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat));
+    rh.gam_timestamp = timestamp;
+}
+
+/** @internal
+* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[in] data Quaternion Adjustment
+* @param[in] timestamp Timestamp of when this is valid
+*/
+void inv_set_compass_correction(const long *data, inv_time_t timestamp)
+{
+    rh.status |= INV_COMPASS_CORRECTION_SET;
+    memcpy(rh.compass_correction, data, sizeof(rh.compass_correction));
+    rh.nav_timestamp = timestamp;
+}
+
+/** @internal
+* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[out] data Quaternion Adjustment
+* @param[out] timestamp Timestamp of when this is valid
+*/
+void inv_get_compass_correction(long *data, inv_time_t *timestamp)
+{
+    memcpy(data, rh.compass_correction, sizeof(rh.compass_correction));
+    *timestamp = rh.nav_timestamp;
+}
+
+/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable.
+ * @return Returns non-zero if there is a large magnetic field.
+ */
+int inv_get_large_mag_field()
+{
+    return rh.large_mag_field;
+}
+
+/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable.
+ * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large.
+ */
+void inv_set_large_mag_field(int state)
+{
+    rh.large_mag_field = state;
+}
+
+/** Gets the accel state set by inv_set_acc_state()
+ * @return accel state.
+ */
+int inv_get_acc_state()
+{
+    return rh.acc_state;
+}
+
+/** Sets the accel state. See inv_get_acc_state() to get the value.
+ * @param[in] state value to set accel state to.
+ */
+void inv_set_acc_state(int state)
+{
+    rh.acc_state = state;
+    return;
+}
+
+/** Returns the motion state
+* @param[out] cntr Number of previous times a no motion event has occured in a row.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+int inv_get_motion_state(unsigned int *cntr)
+{
+    *cntr = rh.motion_state_counter;
+    return rh.motion_state;
+}
+
+/** Sets the motion state
+ * @param[in] state motion state where INV_NO_MOTION is not moving
+ *            and INV_MOTION is moving.
+ */
+void inv_set_motion_state(unsigned char state)
+{
+    long set;
+    if (state == rh.motion_state) {
+        if (state == INV_NO_MOTION) {
+            rh.motion_state_counter++;
+        } else {
+            rh.motion_state_counter = 0;
+        }
+        return;
+    }
+    rh.motion_state_counter = 0;
+    rh.motion_state = state;
+    /* Equivalent to set = state, but #define's may change. */
+    if (state == INV_MOTION)
+        set = INV_MSG_MOTION_EVENT;
+    else
+        set = INV_MSG_NO_MOTION_EVENT;
+    inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0);
+}
+
+/** Sets the local earth's magnetic field
+* @param[in] data Local earth's magnetic field in uT scaled by 2^16.
+*            Length = 3. Y typically points north, Z typically points down in
+*                        northern hemisphere and up in southern hemisphere.
+*/
+void inv_set_local_field(const long *data)
+{
+    memcpy(rh.local_field, data, sizeof(rh.local_field));
+}
+
+/** Gets the local earth's magnetic field
+* @param[out] data Local earth's magnetic field in uT scaled by 2^16.
+*            Length = 3. Y typically points north, Z typically points down in
+*                        northern hemisphere and up in southern hemisphere.
+*/
+void inv_get_local_field(long *data)
+{
+    memcpy(data, rh.local_field, sizeof(rh.local_field));
+}
+
+/** Sets the compass sensitivity
+ * @param[in] data Length 3, sensitivity for each compass axis
+ *  scaled such that 1.0 = 2^30.
+ */
+void inv_set_mag_scale(const long *data)
+{
+    memcpy(rh.mag_scale, data, sizeof(rh.mag_scale));
+}
+
+/** Gets the compass sensitivity
+ * @param[out] data Length 3, sensitivity for each compass axis
+ *  scaled such that 1.0 = 2^30.
+ */
+void inv_get_mag_scale(long *data)
+{
+    memcpy(data, rh.mag_scale, sizeof(rh.mag_scale));
+}
+
+/** Gets gravity vector
+ * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_gravity(long *data)
+{
+    data[0] =
+        inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]);
+    data[1] =
+        inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]);
+    data[2] =
+        (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) -
+        1073741824L;
+
+    return INV_SUCCESS;
+}
+
+/** Returns a quaternion based only on gyro and accel.
+ * @param[out] data 6-axis  gyro and accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_6axis_quaternion(long *data)
+{
+    memcpy(data, rh.gam_quat, sizeof(rh.gam_quat));
+    return INV_SUCCESS;
+}
+
+/** Returns a quaternion.
+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion(long *data)
+{
+    if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) {
+        inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat);
+        rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET);
+    }
+    memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));
+    return INV_SUCCESS;
+}
+
+/** Returns a quaternion.
+ * @param[out] data 9-axis quaternion.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion_float(float *data)
+{
+    long ldata[4];
+    inv_error_t result = inv_get_quaternion(ldata);
+    data[0] = inv_q30_to_float(ldata[0]);
+    data[1] = inv_q30_to_float(ldata[1]);
+    data[2] = inv_q30_to_float(ldata[2]);
+    data[3] = inv_q30_to_float(ldata[3]);
+    return result;
+}
+
+/** Returns a quaternion with accuracy and timestamp.
+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
+ * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate.
+ * @param[out] timestamp Timestamp of this quaternion in nanoseconds
+ */
+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+    inv_get_quaternion(data);
+    *timestamp = inv_get_last_timestamp();
+    if (inv_get_compass_on()) {
+        *accuracy = inv_get_mag_accuracy();
+    } else if (inv_get_gyro_on()) {
+        *accuracy = inv_get_gyro_accuracy();
+    }else if (inv_get_accel_on()) {
+        *accuracy = inv_get_accel_accuracy();
+    } else {
+        *accuracy = 0;
+    }
+}
+
+/** Callback that gets called everytime there is new data. It is 
+ * registered by inv_start_results_holder().
+ * @param[in] sensor_cal New sensor data to process.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal)
+{
+    rh.sensor = sensor_cal;
+    return INV_SUCCESS;
+}
+
+/** Function to turn on this module. This is automatically called by
+ *  inv_enable_results_holder(). Typically not called by users.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_start_results_holder(void)
+{
+    inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER,
+        INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
+    return INV_SUCCESS;
+}
+
+/** Initializes results holder. This is called automatically by the
+* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_results_holder(void)
+{
+    memset(&rh, 0, sizeof(rh));
+    rh.mag_scale[0] = 1L<<30;
+    rh.mag_scale[1] = 1L<<30;
+    rh.mag_scale[2] = 1L<<30;
+    rh.compass_correction[0] = 1L<<30;
+    rh.gam_quat[0] = 1L<<30;
+    rh.nav_quat[0] = 1L<<30;
+    rh.quat_confidence_interval = (float)M_PI;
+    return INV_SUCCESS;
+}
+
+/** Turns on storage of results.
+*/
+inv_error_t inv_enable_results_holder()
+{
+    inv_error_t result;
+    result = inv_init_results_holder();
+    if ( result ) {
+        return result;
+    }
+
+    result = inv_register_mpl_start_notification(inv_start_results_holder);
+    return result;
+}
+
+/** 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()
+ */
+int inv_got_compass_bias()
+{
+    return rh.got_compass_bias;
+}
+
+/** Sets whether we know the compass bias
+ * @param[in] state Set to 1 if we know the compass bias. 
+ *            Can be retrieved with inv_got_compass_bias()
+ */
+void inv_set_compass_bias_found(int state)
+{
+    rh.got_compass_bias = state;
+}
+
+/** Sets the compass state.
+ * @param[in] state Compass state. It can be retrieved with inv_get_compass_state().
+ */
+void inv_set_compass_state(int state)
+{
+    rh.compass_state = state;
+}
+
+/** Get's the compass state
+ * @return the compass state that was set with inv_set_compass_state()
+ */
+int inv_get_compass_state()
+{
+    return rh.compass_state;
+}
+
+/** Set compass bias error. See inv_get_compass_bias_error()
+ * @param[in] bias_error Set's how accurate we know the compass bias. It is the 
+ * error squared.
+ */
+void inv_set_compass_bias_error(const long *bias_error)
+{
+    memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error));
+}
+
+/** Get's compass bias error. See inv_set_compass_bias_error() for setting.
+ * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared.
+ */
+void inv_get_compass_bias_error(long *bias_error)
+{
+    memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error));
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer data in body frame
+ *                with gravity removed
+ *  @param[out] data    3-element vector of accelerometer data in body frame
+ *                with gravity removed
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel(long *data)
+{
+    long gravity[3];
+
+    if (data != NULL)
+    {
+        inv_get_accel_set(data, NULL, NULL);
+        inv_get_gravity(gravity);
+        data[0] -= gravity[0] >> 14;
+        data[1] -= gravity[1] >> 14;
+        data[2] -= gravity[2] >> 14;
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer data in body frame
+ *  @param[out] data    3-element vector of accelerometer data in body frame
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel(long *data)
+{
+    if (data != NULL) {
+        inv_get_accel_set(data, NULL, NULL);
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of accelerometer float data
+ *  @param[out] data    3-element vector of accelerometer float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL && !inv_get_accel(tdata)) {
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ *  @brief      Returns 3-element vector of gyro float data
+ *  @param[out] data    3-element vector of gyro float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_gyro_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL) {
+        inv_get_gyro_set(tdata, NULL, NULL);
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/** Set 9 axis 95% heading confidence interval for quaternion
+* @param[in] ci Confidence interval in radians.
+*/
+void inv_set_heading_confidence_interval(float ci)
+{
+    rh.quat_confidence_interval = ci;
+}
+
+/** Get 9 axis 95% heading confidence interval for quaternion
+* @return Confidence interval in radians.
+*/
+float inv_get_heading_confidence_interval(void)
+{
+    return rh.quat_confidence_interval;
+}
+
+/**
+ *  @brief      Returns 3-element vector of linear accel float data
+ *  @param[out] data    3-element vector of linear aceel float data
+ *  @return     INV_SUCCESS if successful
+ *              INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel_float(float *data)
+{
+    long tdata[3];
+    unsigned char i;
+
+    if (data != NULL && !inv_get_linear_accel(tdata)) {
+        for (i = 0; i < 3; ++i) {
+            data[i] = ((float)tdata[i] / (1L << 16));
+        }
+        return INV_SUCCESS;
+    }
+    else {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors/software/core/mllite/results_holder.h b/libsensors/software/core/mllite/results_holder.h
new file mode 100644
index 0000000..a60d7f0
--- /dev/null
+++ b/libsensors/software/core/mllite/results_holder.h
@@ -0,0 +1,77 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_RESULTS_HOLDER_H__
+#define INV_RESULTS_HOLDER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define INV_MOTION                       0x0001
+#define INV_NO_MOTION                    0x0002
+
+    /**************************************************************************/
+    /*  The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 =   */
+    /*  2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even.               */
+    /*  The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 =        */
+    /*  2^ACC_MAG_SQR_SHIFT                                                   */
+    /**************************************************************************/
+#define ACC_MAG_SQR_SHIFT 16
+
+void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp);
+
+// States
+#define SF_NORMAL 0
+#define SF_UNCALIBRATED 1
+#define SF_STARTUP_SETTLE 2
+#define SF_FAST_SETTLE 3
+#define SF_DISTURBANCE 4
+#define SF_SLOW_SETTLE 5
+
+int inv_get_acc_state();
+void inv_set_acc_state(int state);
+int inv_get_motion_state(unsigned int *cntr);
+void inv_set_motion_state(unsigned char state);
+inv_error_t inv_get_gravity(long *data);
+inv_error_t inv_get_6axis_quaternion(long *data);
+inv_error_t inv_get_quaternion(long *data);
+inv_error_t inv_get_quaternion_float(float *data);
+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp);
+
+inv_error_t inv_enable_results_holder();
+inv_error_t inv_init_results_holder(void);
+
+/* Magnetic Field Parameters*/
+void inv_set_local_field(const long *data);
+void inv_get_local_field(long *data);
+void inv_set_mag_scale(const long *data);
+void inv_get_mag_scale(long *data);
+void inv_set_compass_correction(const long *data, inv_time_t timestamp);
+void inv_get_compass_correction(long *data, inv_time_t *timestamp);
+int inv_got_compass_bias();
+void inv_set_compass_bias_found(int state);
+int inv_get_large_mag_field();
+void inv_set_large_mag_field(int state);
+void inv_set_compass_state(int state);
+int inv_get_compass_state();
+void inv_set_compass_bias_error(const long *bias_error);
+void inv_get_compass_bias_error(long *bias_error);
+inv_error_t inv_get_linear_accel(long *data);
+inv_error_t inv_get_accel(long *data);
+inv_error_t inv_get_accel_float(float *data);
+inv_error_t inv_get_gyro_float(float *data);
+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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_RESULTS_HOLDER_H__
diff --git a/libsensors/software/core/mllite/start_manager.c b/libsensors/software/core/mllite/start_manager.c
new file mode 100644
index 0000000..fb758e7
--- /dev/null
+++ b/libsensors/software/core/mllite/start_manager.c
@@ -0,0 +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;

+}

+

+/**

+ * @}

+ */

diff --git a/libsensors/software/core/mllite/start_manager.h b/libsensors/software/core/mllite/start_manager.h
new file mode 100644
index 0000000..899e3f5
--- /dev/null
+++ b/libsensors/software/core/mllite/start_manager.h
@@ -0,0 +1,27 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_START_MANAGER_H__
+#define INV_START_MANAGER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+
+/** Max number of start callbacks we can handle. */
+#define INV_MAX_START_CB 20
+
+inv_error_t inv_init_start_manager(void);
+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void));
+inv_error_t inv_execute_mpl_start_notification(void);
+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void));
+
+#ifdef __cplusplus
+}
+#endif
+#endif  // INV_START_MANAGER_H__
diff --git a/libsensors/software/core/mllite/storage_manager.c b/libsensors/software/core/mllite/storage_manager.c
new file mode 100644
index 0000000..4b92bfc
--- /dev/null
+++ b/libsensors/software/core/mllite/storage_manager.c
@@ -0,0 +1,200 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  Storage_Manager storage_manager
+ *   @brief     Motion Library - Stores Data for functions.
+ *
+ *
+ *   @{
+ *       @file storage_manager.c
+ *       @brief Load and Store Manager.
+ */
+#include "storage_manager.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+
+/* Must be changed if the format of storage changes */
+#define DEFAULT_KEY 29681
+
+typedef inv_error_t (*load_func_t)(const unsigned char *data);
+typedef inv_error_t (*save_func_t)(unsigned char *data);
+/** Max number of entites that can be stored */
+#define NUM_STORAGE_BOXES 20
+
+struct data_header_t {
+    long size;
+    uint32_t checksum;
+    unsigned int key;
+};
+
+struct data_storage_t {
+    int num; /**< Number of differnt save entities */
+    size_t total_size; /**< Size in bytes to store non volatile data */
+    load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */
+    save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */
+    struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */
+};
+static struct data_storage_t ds;
+
+/** Should be called once before using any of the storage methods. Typically
+* called first by inv_init_mpl().*/
+void inv_init_storage_manager()
+{
+    memset(&ds, 0, sizeof(ds));
+    ds.total_size = sizeof(struct data_header_t);
+}
+
+/** Used to register your mechanism to load and store non-volative data. This should typical be
+* called during the enable function for your feature.
+* @param[in] load_func function pointer you will use to receive data that was stored for you.
+* @param[in] save_func function pointer you will use to save any data you want saved to
+*            non-volatile memory between runs.
+* @param[in] size The size in bytes of the amount of data you want loaded and saved.
+* @param[in] key The key associated with your data type should be unique across MPL.
+*                    The key should change when your type of data for storage changes.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data),
+                                    inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key)
+{
+    int kk;
+    // Check if this has been registered already
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+    }
+    // Make sure there is room
+    if (ds.num >= NUM_STORAGE_BOXES) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    // Add to list
+    ds.hd[ds.num].key = key;
+    ds.hd[ds.num].size = size;
+    ds.load[ds.num] = load_func;
+    ds.save[ds.num] = save_func;
+    ds.total_size += size + sizeof(struct data_header_t);
+    ds.num++;
+
+    return INV_SUCCESS;
+}
+
+/** Returns the memory size needed to perform a store
+* @param[out] size Size in bytes of memory needed to store.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_get_mpl_state_size(size_t *size)
+{
+    *size = ds.total_size;
+    return INV_SUCCESS;
+}
+
+/** @internal
+ * Finds key in ds.hd[] array and returns location
+ * @return location where key exists in array, -1 if not found.
+ */
+static int inv_find_entry(unsigned int key)
+{
+    int kk;
+    for (kk=0; kk<ds.num; ++kk) {
+        if (key == ds.hd[kk].key) {
+            return kk;
+        }
+    }
+    return -1;
+}
+
+/** This function takes a block of data that has been saved in non-volatile memory and pushes
+* to the proper locations. Multiple error checks are performed on the data.
+* @param[in] data Data that was saved to be loaded up by MPL
+* @param[in] length Length of data vector in bytes
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length)
+{
+    struct data_header_t *hd;
+    int entry;
+    uint32_t checksum;
+    long len;
+
+    len = length; // Important so we get negative numbers
+    if (len < sizeof(struct data_header_t))
+        return INV_ERROR_CALIBRATION_LOAD;	// No data
+    hd = (struct data_header_t *)data;
+    if (hd->key != DEFAULT_KEY)
+        return INV_ERROR_CALIBRATION_LOAD;	// Key changed or data corruption
+    len = MIN(hd->size, len);
+    len = hd->size;
+    len -= sizeof(struct data_header_t);
+    data += sizeof(struct data_header_t);
+    checksum = inv_checksum(data, len);
+    if (checksum != hd->checksum)
+        return INV_ERROR_CALIBRATION_LOAD;	// Data corruption
+
+    while (len > (long)sizeof(struct data_header_t)) {
+        hd = (struct data_header_t *)data;
+        entry = inv_find_entry(hd->key);
+        data += sizeof(struct data_header_t);
+        len -= sizeof(struct data_header_t);
+        if (entry >= 0 && len >= hd->size) {
+            if (hd->size == ds.hd[entry].size) {
+                checksum = inv_checksum(data, hd->size);
+                if (checksum == hd->checksum) {
+                    ds.load[entry](data);
+                } else {
+                    return INV_ERROR_CALIBRATION_LOAD;
+                }
+            }
+        }
+        len -= hd->size;
+        if (len >= 0)
+            data = data + hd->size;
+    }
+
+    return INV_SUCCESS;
+}
+
+/** This function fills up a block of memory to be stored in non-volatile memory.
+* @param[out] data Place to store data, size of sz, must be at least size
+*                  returned by inv_get_mpl_state_size()
+* @param[in] sz Size of data.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz)
+{
+    unsigned char *cur;
+    int kk;
+    struct data_header_t *hd;
+
+    if (sz >= ds.total_size) {
+        cur = data + sizeof(struct data_header_t);
+        for (kk = 0; kk < ds.num; ++kk) {
+            hd = (struct data_header_t *)cur;
+            cur += sizeof(struct data_header_t);
+            ds.save[kk](cur);
+            hd->checksum = inv_checksum(cur, ds.hd[kk].size);
+            hd->size = ds.hd[kk].size;
+            hd->key = ds.hd[kk].key;
+            cur += ds.hd[kk].size;
+        }
+    } else {
+        return INV_ERROR_CALIBRATION_LOAD;
+    }
+
+    hd = (struct data_header_t *)data;
+    hd->checksum = inv_checksum(data + sizeof(struct data_header_t),
+                                ds.total_size - sizeof(struct data_header_t));
+    hd->key = DEFAULT_KEY;
+    hd->size = ds.total_size;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors/software/core/mllite/storage_manager.h b/libsensors/software/core/mllite/storage_manager.h
new file mode 100644
index 0000000..7255591
--- /dev/null
+++ b/libsensors/software/core/mllite/storage_manager.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_STORAGE_MANAGER_H__
+#define INV_STORAGE_MANAGER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_register_load_store(
+                           inv_error_t (*load_func)(const unsigned char *data),
+                           inv_error_t (*save_func)(unsigned char *data), 
+                           size_t size, unsigned int key);
+void inv_init_storage_manager(void);
+
+inv_error_t inv_get_mpl_state_size(size_t *size);
+inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len);
+inv_error_t inv_save_mpl_states(unsigned char *data, size_t len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_STORAGE_MANAGER_H__ */
diff --git a/libsensors/software/core/mpl/accel_auto_cal.h b/libsensors/software/core/mpl/accel_auto_cal.h
new file mode 100644
index 0000000..5a53213
--- /dev/null
+++ b/libsensors/software/core/mpl/accel_auto_cal.h
@@ -0,0 +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__

+

diff --git a/libsensors/software/core/mpl/adv_func.h b/libsensors/software/core/mpl/adv_func.h
new file mode 100644
index 0000000..3f8595f
--- /dev/null
+++ b/libsensors/software/core/mpl/adv_func.h
@@ -0,0 +1,30 @@
+/*
+ $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/software/core/mpl/authenticate.h b/libsensors/software/core/mpl/authenticate.h
new file mode 100644
index 0000000..d7c803b
--- /dev/null
+++ b/libsensors/software/core/mpl/authenticate.h
@@ -0,0 +1,21 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_AUTHENTICATE_H__
+#define MLDMP_AUTHENTICATE_H__
+
+#include "invensense.h"
+
+inv_error_t inv_check_key(void);
+
+#endif  /* MLDMP_AUTHENTICATE_H__ */
diff --git a/libsensors/software/core/mpl/auto_calibration.h b/libsensors/software/core/mpl/auto_calibration.h
new file mode 100644
index 0000000..3dd9827
--- /dev/null
+++ b/libsensors/software/core/mpl/auto_calibration.h
@@ -0,0 +1,33 @@
+/*
+ $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/software/core/mpl/build/android/libmplmpu.so b/libsensors/software/core/mpl/build/android/libmplmpu.so
new file mode 100644
index 0000000..116b618
--- /dev/null
+++ b/libsensors/software/core/mpl/build/android/libmplmpu.so
diff --git a/libsensors/software/core/mpl/build/android/shared.mk b/libsensors/software/core/mpl/build/android/shared.mk
new file mode 100644
index 0000000..79cf9c1
--- /dev/null
+++ b/libsensors/software/core/mpl/build/android/shared.mk
@@ -0,0 +1,92 @@
+MPL_LIB_NAME ?= mplmpu
+LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+MPL_DIR  = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
+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
+
+####################################################################################################
+## sources
+
+INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES, VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all mpl clean cleanall
+
+all: mpl
+
+mpl: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors/software/core/mpl/build/android/static.mk b/libsensors/software/core/mpl/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors/software/core/mpl/build/android/static.mk
@@ -0,0 +1,110 @@
+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/software/core/mpl/build/filelist.mk b/libsensors/software/core/mpl/build/filelist.mk
new file mode 100644
index 0000000..e18003a
--- /dev/null
+++ b/libsensors/software/core/mpl/build/filelist.mk
@@ -0,0 +1,46 @@
+#### filelist.mk for mpl ####
+
+# headers only
+HEADERS := $(MPL_DIR)/mpu.h
+
+# headers for sources
+HEADERS := $(MPL_DIR)/fast_no_motion.h
+HEADERS += $(MPL_DIR)/fusion_9axis.h
+HEADERS += $(MPL_DIR)/motion_no_motion.h
+HEADERS += $(MPL_DIR)/no_gyro_fusion.h
+HEADERS += $(MPL_DIR)/quaternion_supervisor.h
+HEADERS += $(MPL_DIR)/gyro_tc.h
+HEADERS += $(MPL_DIR)/authenticate.h
+HEADERS += $(MPL_DIR)/accel_auto_cal.h
+HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h
+HEADERS += $(MPL_DIR)/compass_vec_cal.h
+HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h
+HEADERS += $(MPL_DIR)/mag_disturb.h
+HEADERS += $(MPL_DIR)/mag_disturb_protected.h
+HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h
+HEADERS += $(MPL_DIR)/heading_from_gyro.h
+HEADERS += $(MPL_DIR)/compass_fit.h
+HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h
+#HEADERS += $(MPL_DIR)/auto_calibration.h
+
+# sources
+SOURCES := $(MPL_DIR)/fast_no_motion.c
+SOURCES += $(MPL_DIR)/fusion_9axis.c
+SOURCES += $(MPL_DIR)/motion_no_motion.c
+SOURCES += $(MPL_DIR)/no_gyro_fusion.c
+SOURCES += $(MPL_DIR)/quaternion_supervisor.c
+SOURCES += $(MPL_DIR)/gyro_tc.c
+SOURCES += $(MPL_DIR)/authenticate.c
+SOURCES += $(MPL_DIR)/accel_auto_cal.c
+SOURCES += $(MPL_DIR)/compass_vec_cal.c
+SOURCES += $(MPL_DIR)/mag_disturb.c
+SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c
+SOURCES += $(MPL_DIR)/heading_from_gyro.c
+SOURCES += $(MPL_DIR)/compass_fit.c
+SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c
+#SOURCES += $(MPL_DIR)/auto_calibration.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH = $(MPL_DIR)
+
diff --git a/libsensors/software/core/mpl/compass_bias_w_gyro.h b/libsensors/software/core/mpl/compass_bias_w_gyro.h
new file mode 100644
index 0000000..4f01fc2
--- /dev/null
+++ b/libsensors/software/core/mpl/compass_bias_w_gyro.h
@@ -0,0 +1,31 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_COMPASSBIASWGYRO_H__
+#define MLDMP_COMPASSBIASWGYRO_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_compass_bias_w_gyro();
+    inv_error_t inv_disable_compass_bias_w_gyro();
+    void inv_init_compass_bias_w_gyro();
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_COMPASSBIASWGYRO_H__
diff --git a/libsensors/software/core/mpl/compass_fit.h b/libsensors/software/core/mpl/compass_fit.h
new file mode 100644
index 0000000..be3cce8
--- /dev/null
+++ b/libsensors/software/core/mpl/compass_fit.h
@@ -0,0 +1,28 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+#ifndef INV_MLDMP_COMPASSFIT_H__
+#define INV_MLDMP_COMPASSFIT_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void inv_init_compass_fit(void);
+inv_error_t inv_enable_compass_fit(void);
+inv_error_t inv_disable_compass_fit(void);
+inv_error_t inv_start_compass_fit(void);
+inv_error_t inv_stop_compass_fit(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // INV_MLDMP_COMPASSFIT_H__
diff --git a/libsensors/software/core/mpl/compass_vec_cal.h b/libsensors/software/core/mpl/compass_vec_cal.h
new file mode 100644
index 0000000..a3e044c
--- /dev/null
+++ b/libsensors/software/core/mpl/compass_vec_cal.h
@@ -0,0 +1,34 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef COMPASS_ONLY_CAL_H__
+#define COMPASS_ONLY_CAL_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_vector_compass_cal();
+inv_error_t inv_disable_vector_compass_cal();
+inv_error_t inv_start_vector_compass_cal(void);
+inv_error_t inv_stop_vector_compass_cal(void);
+void inv_vector_compass_cal_sensitivity(float sens);
+inv_error_t inv_init_vector_compass_cal();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // COMPASS_ONLY_CAL_H__
diff --git a/libsensors/software/core/mpl/fast_no_motion.h b/libsensors/software/core/mpl/fast_no_motion.h
new file mode 100644
index 0000000..2a33093
--- /dev/null
+++ b/libsensors/software/core/mpl/fast_no_motion.h
@@ -0,0 +1,44 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_FAST_NO_MOTION_H__
+#define MLDMP_FAST_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_fast_nomot(void);
+    inv_error_t inv_disable_fast_nomot(void);
+    inv_error_t inv_start_fast_nomot(void);

+    inv_error_t inv_stop_fast_nomot(void);
+    inv_error_t inv_init_fast_nomot(void);
+    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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FAST_NO_MOTION_H__
+
diff --git a/libsensors/software/core/mpl/fusion_9axis.h b/libsensors/software/core/mpl/fusion_9axis.h
new file mode 100644
index 0000000..616694a
--- /dev/null
+++ b/libsensors/software/core/mpl/fusion_9axis.h
@@ -0,0 +1,35 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_FUSION9AXIS_H__
+#define MLDMP_FUSION9AXIS_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void inv_init_9x_fusion(void);
+    inv_error_t inv_9x_fusion_state_change(unsigned char newState);
+    inv_error_t inv_enable_9x_sensor_fusion(void);
+    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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/libsensors/software/core/mpl/gyro_tc.h b/libsensors/software/core/mpl/gyro_tc.h
new file mode 100644
index 0000000..8f1d50e
--- /dev/null
+++ b/libsensors/software/core/mpl/gyro_tc.h
@@ -0,0 +1,43 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _GYRO_TC_H
+#define _GYRO_TC_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_gyro_tc(void);
+inv_error_t inv_disable_gyro_tc(void);
+inv_error_t inv_start_gyro_tc(void);

+inv_error_t inv_stop_gyro_tc(void);
+
+inv_error_t inv_get_gyro_ts(long *data);
+inv_error_t inv_set_gyro_ts(long *data);
+
+inv_error_t inv_init_gyro_ts(void);
+
+inv_error_t inv_set_gtc_max_temp(long data);
+inv_error_t inv_set_gtc_min_temp(long data);
+
+inv_error_t inv_print_gtc_data(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* _GYRO_TC_H */
+
diff --git a/libsensors/software/core/mpl/heading_from_gyro.h b/libsensors/software/core/mpl/heading_from_gyro.h
new file mode 100644
index 0000000..09ecc42
--- /dev/null
+++ b/libsensors/software/core/mpl/heading_from_gyro.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _HEADING_FROM_GYRO_H_
+#define _HEADING_FROM_GYRO_H_
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_heading_from_gyro(void);
+    inv_error_t inv_disable_heading_from_gyro(void);
+    void inv_init_heading_from_gyro(void);
+    inv_error_t inv_start_heading_from_gyro(void);
+    inv_error_t inv_stop_heading_from_gyro(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  /* _HEADING_FROM_GYRO_H_ */
diff --git a/libsensors/software/core/mpl/interpolator.h b/libsensors/software/core/mpl/interpolator.h
new file mode 100644
index 0000000..5eb571d
--- /dev/null
+++ b/libsensors/software/core/mpl/interpolator.h
@@ -0,0 +1,103 @@
+/*
+ $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/software/core/mpl/inv_log.h b/libsensors/software/core/mpl/inv_log.h
new file mode 100644
index 0000000..972844b
--- /dev/null
+++ b/libsensors/software/core/mpl/inv_log.h
@@ -0,0 +1,7 @@
+#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/software/core/mpl/inv_math.h b/libsensors/software/core/mpl/inv_math.h
new file mode 100644
index 0000000..6620bbf
--- /dev/null
+++ b/libsensors/software/core/mpl/inv_math.h
@@ -0,0 +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>

diff --git a/libsensors/software/core/mpl/invensense_adv.h b/libsensors/software/core/mpl/invensense_adv.h
new file mode 100644
index 0000000..9e59c18
--- /dev/null
+++ b/libsensors/software/core/mpl/invensense_adv.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/*
+    Main header file for Invensense's Advanced library.
+*/
+
+#include "accel_auto_cal.h"
+#include "compass_bias_w_gyro.h"
+#include "compass_fit.h"
+#include "compass_vec_cal.h"
+#include "fast_no_motion.h"
+#include "fusion_9axis.h"
+#include "gyro_tc.h"
+#include "heading_from_gyro.h"
+#include "mag_disturb.h"
+#include "motion_no_motion.h"
+#include "no_gyro_fusion.h"
+#include "quaternion_supervisor.h"
+#include "mag_disturb.h"
+#include "quat_accuracy_monitor.h"
diff --git a/libsensors/software/core/mpl/mag_disturb.h b/libsensors/software/core/mpl/mag_disturb.h
new file mode 100644
index 0000000..38df919
--- /dev/null
+++ b/libsensors/software/core/mpl/mag_disturb.h
@@ -0,0 +1,37 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+#ifndef MLDMP_MAGDISTURB_H__
+#define MLDMP_MAGDISTURB_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat,
+        const long *compass, const long *gravity);
+
+    void inv_track_dip_angle(int mode, float currdip);
+
+    inv_error_t inv_enable_magnetic_disturbance(void);
+    inv_error_t inv_disable_magnetic_disturbance(void);
+    int inv_get_magnetic_disturbance_state();
+    inv_error_t inv_set_magnetic_disturbance(int time_ms);
+    inv_error_t inv_disable_dip_tracking(void);
+    inv_error_t inv_enable_dip_tracking(void);
+    inv_error_t inv_init_magnetic_disturbance(void);

+
+    float Mag3ofNormalizedLong(const long *x);

+

+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_MAGDISTURB_H__
diff --git a/libsensors/software/core/mpl/mlsetinterrupts.h b/libsensors/software/core/mpl/mlsetinterrupts.h
new file mode 100644
index 0000000..a81dabb
--- /dev/null
+++ b/libsensors/software/core/mpl/mlsetinterrupts.h
@@ -0,0 +1,23 @@
+/*
+ $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/software/core/mpl/mlsupervisor_9axis.h b/libsensors/software/core/mpl/mlsupervisor_9axis.h
new file mode 100644
index 0000000..3779381
--- /dev/null
+++ b/libsensors/software/core/mpl/mlsupervisor_9axis.h
@@ -0,0 +1,57 @@
+/*
+ $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/software/core/mpl/motion_no_motion.h b/libsensors/software/core/mpl/motion_no_motion.h
new file mode 100644
index 0000000..01cf1c0
--- /dev/null
+++ b/libsensors/software/core/mpl/motion_no_motion.h
@@ -0,0 +1,28 @@
+/*

+ $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__

diff --git a/libsensors/software/core/mpl/no_gyro_fusion.h b/libsensors/software/core/mpl/no_gyro_fusion.h
new file mode 100644
index 0000000..38d5690
--- /dev/null
+++ b/libsensors/software/core/mpl/no_gyro_fusion.h
@@ -0,0 +1,34 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_NOGYROFUSION_H__
+#define MLDMP_NOGYROFUSION_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_no_gyro_fusion(void);
+    inv_error_t inv_disable_no_gyro_fusion(void);
+    inv_error_t inv_start_no_gyro_fusion(void);

+    inv_error_t inv_start_no_gyro_fusion(void);

+    inv_error_t inv_init_no_gyro_fusion(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_NOGYROFUSION_H__
diff --git a/libsensors/software/core/mpl/orientation.h b/libsensors/software/core/mpl/orientation.h
new file mode 100644
index 0000000..ab4e45e
--- /dev/null
+++ b/libsensors/software/core/mpl/orientation.h
@@ -0,0 +1,42 @@
+/*
+ $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/software/core/mpl/progressive_no_motion.h b/libsensors/software/core/mpl/progressive_no_motion.h
new file mode 100644
index 0000000..99333e3
--- /dev/null
+++ b/libsensors/software/core/mpl/progressive_no_motion.h
@@ -0,0 +1,39 @@
+/*
+ $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/software/core/mpl/quat_accuracy_monitor.h b/libsensors/software/core/mpl/quat_accuracy_monitor.h
new file mode 100644
index 0000000..2cf7a50
--- /dev/null
+++ b/libsensors/software/core/mpl/quat_accuracy_monitor.h
@@ -0,0 +1,70 @@
+/*

+	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__

diff --git a/libsensors/software/core/mpl/quaternion_supervisor.h b/libsensors/software/core/mpl/quaternion_supervisor.h
new file mode 100644
index 0000000..532e8af
--- /dev/null
+++ b/libsensors/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,26 @@
+/*

+ $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__

diff --git a/libsensors/software/core/mpl/sensor_moments.h b/libsensors/software/core/mpl/sensor_moments.h
new file mode 100644
index 0000000..73eb363
--- /dev/null
+++ b/libsensors/software/core/mpl/sensor_moments.h
@@ -0,0 +1,42 @@
+/*
+ $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/software/core/mpl/state_storage.h b/libsensors/software/core/mpl/state_storage.h
new file mode 100644
index 0000000..c1eb47b
--- /dev/null
+++ b/libsensors/software/core/mpl/state_storage.h
@@ -0,0 +1,25 @@
+/*
+ $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/software/simple_apps/common/external_hardware.h b/libsensors/software/simple_apps/common/external_hardware.h
new file mode 100644
index 0000000..55e3b20
--- /dev/null
+++ b/libsensors/software/simple_apps/common/external_hardware.h
@@ -0,0 +1,156 @@
+/*
+    Accelerometer
+*/
+#define get_accel_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_ADXL34X	/* ADI accelerometer */
+struct ext_slave_descr *adxl34x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr adxl34x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA150	/* Bosch accelerometer */
+struct ext_slave_descr *bma150_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma150_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA222	/* Bosch 222 accelerometer */
+struct ext_slave_descr *bma222_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma222_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA250	/* Bosch accelerometer */
+struct ext_slave_descr *bma250_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma250_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXSD9	/* Kionix accelerometer */
+struct ext_slave_descr *kxsd9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxsd9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXTF9	/* Kionix accelerometer */
+struct ext_slave_descr *kxtf9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxtf9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LIS331DLH	/* ST accelerometer */
+struct ext_slave_descr *lis331_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis331_get_slave_descr
+#endif
+
+
+#ifdef CONFIG_MPU_SENSORS_LIS3DH	/* ST accelerometer */
+struct ext_slave_descr *lis3dh_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis3dh_get_slave_descr
+#endif
+
+/* ST accelerometer in LSM303DLx combo */
+#if defined CONFIG_MPU_SENSORS_LSM303DLX_A
+struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lsm303dlx_a_get_slave_descr
+#endif
+
+/* MPU6050 Accel */
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+struct ext_slave_descr *mpu6050_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mpu6050_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA8450	/* Freescale accelerometer */
+struct ext_slave_descr *mma8450_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma8450_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA845X	/* Freescale accelerometer */
+struct ext_slave_descr *mma845x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma845x_get_slave_descr
+#endif
+
+
+/*
+    Compass
+*/
+#define get_compass_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_AK8975	/* AKM compass */
+struct ext_slave_descr *ak8975_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ak8975_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI30X	/* AICHI Steel  AMI304/305 compass */
+struct ext_slave_descr *ami30x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami30x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI306	/* AICHI Steel AMI306 compass */
+struct ext_slave_descr *ami306_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami306_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HMC5883	/* Honeywell compass */
+struct ext_slave_descr *hmc5883_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hmc5883_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMC314X	/* MEMSIC compass */
+struct ext_slave_descr *mmc314x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr mmc314x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M	/* ST compass */
+struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr lsm303dlx_m_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS529	/* Yamaha compass */
+struct ext_slave_descr *yas529_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas529_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS530	/* Yamaha compass */
+struct ext_slave_descr *yas530_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas530_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD002B	/* Alps HSCDTD002B compass */
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd002b_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD004A	/* Alps HSCDTD004A compass */
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd004a_get_slave_descr
+#endif
+/*
+    Pressure
+*/
+#define get_pressure_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_BMA085	/* BMA pressure */
+struct ext_slave_descr *bma085_get_slave_descr(void);
+#undef get_pressure_slave_descr
+#define get_pressure_slave_descr bma085_get_slave_descr
+#endif
diff --git a/libsensors/software/simple_apps/common/fopenCMake.c b/libsensors/software/simple_apps/common/fopenCMake.c
new file mode 100644
index 0000000..2936109
--- /dev/null
+++ b/libsensors/software/simple_apps/common/fopenCMake.c
@@ -0,0 +1,56 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <string.h>
+
+#include "fopenCMake.h"
+#include "path_configure.h"
+
+/**
+ *  @brief  Replacement for fopen that concatenates the location of the 
+ *          source tree onto the filename path. 
+ *          It looks in 3 locations:
+ *              - in the current directory, 
+ *              - then it looks in "..", 
+ *              - lastly in the define UNITTEST_SOURCE_DIR which 
+ *                gets defined by CMake.
+ * @param filename 
+ *              Filename relative to base of source directory.
+ * @param prop 
+ *              Second argument to fopen.
+ */
+FILE *fopenCMake(const char *filename, const char *prop)
+{
+    char path[150];
+    FILE *file;
+
+    // Look first in current directory
+    file = fopen(filename, prop);
+    if (file == NULL) {
+        // Now look in ".."
+#ifdef WIN32
+        strcpy(path, "..\\");
+#else
+        strcpy(path, "../");
+#endif
+        strcat(path, filename);
+        file = fopen(path, prop);
+        if (file == NULL) {
+            // Now look in definition by CMake
+            strcpy(path, PATH_SOURCE_DIR);
+            strcat(path, filename);
+            file = fopen(path, prop);
+        }
+    }
+    return file;
+}
+
+
diff --git a/libsensors/software/simple_apps/common/fopenCMake.h b/libsensors/software/simple_apps/common/fopenCMake.h
new file mode 100644
index 0000000..c5eba39
--- /dev/null
+++ b/libsensors/software/simple_apps/common/fopenCMake.h
@@ -0,0 +1,21 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef FOPEN_CMAKE_H__
+#define FOPEN_CMAKE_H__
+
+#include <stdio.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+FILE *fopenCMake( const char *filename, const char *prop );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // FOPEN_CMAKE_H__
diff --git a/libsensors/software/simple_apps/common/gestureMenu.c b/libsensors/software/simple_apps/common/gestureMenu.c
new file mode 100644
index 0000000..2a9487c
--- /dev/null
+++ b/libsensors/software/simple_apps/common/gestureMenu.c
@@ -0,0 +1,725 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $
+ *****************************************************************************/
+
+#define _USE_MATH_DEFINES
+#include <stdio.h>
+#include <stddef.h>
+#include <math.h>
+#include <string.h>
+
+#include "ml.h"
+#include "mlmath.h"
+#include "gesture.h"
+#include "orientation.h"
+#include "gestureMenu.h"
+#include "fifo.h"
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "gest"
+#include "log.h"
+#include "mldl_cfg.h"
+
+static unsigned long sensors[] = {
+    INV_NINE_AXIS,
+    INV_THREE_AXIS_GYRO,
+    INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL,
+    INV_THREE_AXIS_ACCEL,
+    INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS,
+    INV_THREE_AXIS_COMPASS,
+    INV_SIX_AXIS_GYRO_ACCEL,
+    INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS,
+    INV_SIX_AXIS_ACCEL_COMPASS,
+};
+
+static char *sensors_string[] = {
+    "INV_NINE_AXIS",
+    "INV_THREE_AXIS_GYRO",
+    "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL",
+    "INV_THREE_AXIS_ACCEL",
+    "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS",
+    "INV_THREE_AXIS_COMPASS",
+    "INV_SIX_AXIS_GYRO_ACCEL",
+    "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS",
+    "INV_SIX_AXIS_ACCEL_COMPASS",
+};
+
+/**
+ * Prints the menu with the current thresholds
+ *
+ * @param params The parameters to print
+ */
+void PrintGestureMenu(tGestureMenuParams const * const params)
+{
+    MPL_LOGI("Press h at any time to re-display this menu\n");
+    MPL_LOGI("TAP PARAMETERS:\n");
+    MPL_LOGI("    Use LEFT and RIGHT arrows to adjust Tap Time     \n\n");
+    MPL_LOGI("    j          : Increase X threshold     : %5d\n",
+             params->xTapThreshold);
+    MPL_LOGI("    J (Shift-j): Decrease X threshold\n");
+    MPL_LOGI("    k          : Increase Y threshold     : %5d\n",
+             params->yTapThreshold);
+    MPL_LOGI("    K (Shift-k): Decrease Y threshold\n");
+    MPL_LOGI("    i          : Increase Z threshold     : %5d\n",
+             params->zTapThreshold);
+    MPL_LOGI("    I (Shift-i): Decrease Z threshold\n");
+    MPL_LOGI("    l          : Increase tap time        : %5d\n",
+             params->tapTime);
+    MPL_LOGI("    L (Shift-l): Decrease tap time\n");
+    MPL_LOGI("    o          : Increase next tap time   : %5d\n",
+             params->nextTapTime);
+    MPL_LOGI("    O (Shift-o): Increase next tap time\n");
+    MPL_LOGI("    u          : Increase max Taps        : %5d\n",
+             params->maxTaps);
+    MPL_LOGI("    U (Shift-u): Increase max Taps\n");
+
+    MPL_LOGI("SHAKE PARAMETERS:\n");
+    MPL_LOGI("    x          : Increase X threshold     : %5d\n",
+             params->xShakeThresh);
+    MPL_LOGI("    X (Shift-x): Decrease X threshold\n");
+    MPL_LOGI("    y          : Increase Y threshold     : %5d\n",
+             params->yShakeThresh);
+    MPL_LOGI("    Y (Shift-y): Decrease Y threshold\n");
+    MPL_LOGI("    z          : Increase Z threshold     : %5d\n",
+             params->zShakeThresh);
+    MPL_LOGI("    Z (Shift-z): Decrease Z threshold\n");
+    MPL_LOGI("    s          : Toggle Shake Function    : %5d\n",
+             params->shakeFunction);
+    MPL_LOGI("    t          : Increase Shake Time      : %5d\n",
+             params->shakeTime);
+    MPL_LOGI("    T (Shift-T): Decrease Shake Time\n");
+    MPL_LOGI("    n          : Increase Next Shake Time : %5d\n",
+             params->nextShakeTime);
+    MPL_LOGI("    N (Shift-n): Decrease Next Shake Time\n");
+    MPL_LOGI("    m          : Increase max Shakes      : %5d\n",
+             params->maxShakes);
+    MPL_LOGI("    M (Shift-m): Decrease max Shakes\n");
+    MPL_LOGI("SNAP  PARAMETERS:\n");
+    MPL_LOGI("    p          : Increase Pitch threshold : %5d\n",
+             params->xSnapThresh);
+    MPL_LOGI("    P (Shift-p): Decrease Pitch threshold\n");
+    MPL_LOGI("    r          : Increase Roll  threshold : %5d\n",
+             params->ySnapThresh);
+    MPL_LOGI("    R (Shift-r): Decrease Roll  threshold\n");
+    MPL_LOGI("    a          : Increase yAw   threshold : %5d\n",
+             params->zSnapThresh);
+    MPL_LOGI("    A (Shift-a): Decrease yAw   threshold\n");
+    MPL_LOGI("YAW ROTATION PARAMETERS:\n");
+    MPL_LOGI("    e          : Increase yaW Rotate time : %5d\n",
+             params->yawRotateTime);
+    MPL_LOGI("    E (Shift-r): Decrease yaW Rotate time\n");
+    MPL_LOGI("    w          : Increase yaW Rotate threshold : %5d\n",
+             params->yawRotateThreshold);
+    MPL_LOGI("    W (Shift-w): Decrease yaW Rotate threshold\n");
+    MPL_LOGI("ORIENTATION PARAMETER:\n");
+    MPL_LOGI("    d          : Increase orientation angle threshold : %5f\n",
+             params->orientationThreshold);
+    MPL_LOGI("    D (Shift-d): Decrease orientation angle threshold\n");
+    MPL_LOGI("FIFO RATE:\n");
+    MPL_LOGI("    f          : Increase fifo divider    : %5d\n",
+             inv_get_fifo_rate());
+    MPL_LOGI("    F (Shift-f): Decrease fifo divider\n");
+    MPL_LOGI("REQUESTED SENSORS:\n");
+    MPL_LOGI("    S (Shift-s): Toggle in use sensors : %s\n",
+             sensors_string[params->sensorsIndex]);
+    MPL_LOGI("    F (Shift-f): Decrease fifo divider\n");
+
+    /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */
+    /* S is available */
+
+    MPL_LOGI("\n\n");
+}
+
+/**
+ * Handles a keyboard input and updates an appropriate threshold, prints then
+ * menu or returns false if the character is not processed.
+ *
+ * @param params The parameters to modify if the thresholds are updated
+ * @param ch     The input character
+ *
+ * @return        true if the character was processed, false otherwise
+ */
+inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch)
+{
+    int result = INV_SUCCESS;
+    /* Dynamic keyboard processing */
+
+    switch (ch) {
+    case 'j':
+        params->xTapThreshold += 20;
+        // Intentionally fall through
+    case 'J':  {
+        params->xTapThreshold -= 10;
+        if (params->xTapThreshold < 0) params->xTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n",
+                 params->xTapThreshold);
+    } break;
+    case 'k':
+        params->yTapThreshold += 20;
+        // Intentionally fall through
+    case 'K':  {
+        params->yTapThreshold -= 10;
+        if (params->yTapThreshold < 0) params->yTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n",
+                 params->yTapThreshold);
+    } break;
+    case 'i':
+        params->zTapThreshold += 20;
+        // Intentionally fall through
+    case 'I':  {
+        params->zTapThreshold -= 10;
+        if (params->zTapThreshold < 0) params->zTapThreshold = 0;
+        result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n",
+                 params->zTapThreshold);
+    } break;
+
+    case  'l':
+        params->tapTime += 20;
+        // Intentionally fall through
+    case 'L':  {
+        params->tapTime -= 10;
+        if (params->tapTime < 0) params->tapTime = 0;
+        result = inv_set_next_tap_time(params->tapTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime);
+    } break;
+    case  'o':
+        params->nextTapTime += 20;
+        // Intentionally fall through
+    case 'O':  {
+        params->nextTapTime -= 10;
+        if (params->nextTapTime < 0) params->nextTapTime = 0;
+        result = MLSetNextTapTime(params->nextTapTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+        }
+        MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime);
+    } break;
+    case  'u':
+        params->maxTaps += 2;
+        // Intentionally fall through
+    case 'U':  {
+        params->maxTaps -= 1;
+        if (params->maxTaps < 0) params->maxTaps = 0;
+        result = inv_set_max_taps(params->maxTaps);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps);
+    } break;
+    case 's': {
+        int shakeParam;
+        params->shakeFunction = (params->shakeFunction + 1) % 2;
+        switch (params->shakeFunction)
+        {
+        case 0:
+            shakeParam = INV_NO_RETRACTION;
+            MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n");
+            break;
+        case 1:
+            shakeParam = INV_RETRACTION;
+            MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n");
+            break;
+        };
+        result = inv_set_shake_func(shakeParam);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+        }
+    } break;
+    case 'x':
+        params->xShakeThresh += 200;
+        // Intentionally fall through
+    case 'X': {
+        params->xShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh);
+    } break;
+    case 'y':
+        params->yShakeThresh += 200;
+        // Intentionally fall through
+    case 'Y': {
+        params->yShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh);
+    } break;
+    case 'z':
+        params->zShakeThresh += 200;
+        // Intentionally fall through
+    case 'Z':{
+        params->zShakeThresh -= 100;
+        result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh);
+    } break;
+    case 'r':
+        params->ySnapThresh += 20;
+        // Intentionally fall through
+    case 'R': {
+        params->ySnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh);
+    } break;
+    case 'p':
+        params->xSnapThresh += 20;
+        // Intentionally fall through
+    case 'P': {
+        params->xSnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n",
+                 params->xSnapThresh);
+    } break;
+    case 'a':
+        params->zSnapThresh += 20;
+    case 'A': {
+        params->zSnapThresh -= 10;
+        result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh);
+    } break;
+
+    case 't':
+        params->shakeTime += 20;
+    case 'T':{
+        params->shakeTime -= 10;
+        result = inv_set_shake_time(params->shakeTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime);
+    } break;
+    case 'n':
+        params->nextShakeTime += 20;
+    case 'N':{
+        params->nextShakeTime -= 10;
+        result = inv_set_next_shake_time(params->nextShakeTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime);
+    } break;
+    case 'm':
+        params->maxShakes += 2;
+    case 'M':{
+        params->maxShakes -= 1;
+        result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes);
+    } break;
+    case 'e':
+        params->yawRotateTime += 20;
+    case 'E':{
+        params->yawRotateTime -= 10;
+        result = inv_set_yaw_rotate_time(params->yawRotateTime);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime);
+    } break;
+    case 'w':
+        params->yawRotateThreshold += 2;
+    case 'W':{
+        params->yawRotateThreshold -= 1;
+        result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold);
+    } break;
+    case 'c':
+        params->shakeRejectValue += 0.20f;
+    case 'C':{
+        params->shakeRejectValue -= 0.10f;
+        result = inv_set_tap_shake_reject(params->shakeRejectValue);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue);
+    } break;
+    case 'd':
+        params->orientationThreshold += 10;
+    case 'D':{
+        params->orientationThreshold -= 5;
+        if (params->orientationThreshold > 90) {
+            params->orientationThreshold = 90;
+        }
+
+        if (params->orientationThreshold < 0 ) {
+            params->orientationThreshold = 0;
+        }
+
+        result = inv_set_orientation_thresh(params->orientationThreshold,
+                                           5, 80,
+                                           INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+        if (INV_SUCCESS != result) {
+            MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result);
+        }
+        MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d,"
+                 " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n",
+                 params->orientationThreshold, 5, 80);
+    } break;
+    case 'f':
+        result = inv_set_fifo_rate(inv_get_fifo_rate() + 1);
+        MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+        break;
+    case 'F':
+    {
+        unsigned short newRate = inv_get_fifo_rate();
+        if (newRate > 0)
+            newRate--;
+        result = inv_set_fifo_rate(newRate);
+        MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+        break;
+    }
+    case 'S':
+        params->sensorsIndex++;
+        if (params->sensorsIndex >= ARRAY_SIZE(sensors)) {
+            params->sensorsIndex = 0;
+        }
+        result = inv_set_mpu_sensors(
+            sensors[params->sensorsIndex] & params->available_sensors);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result,
+                 sensors_string[params->sensorsIndex]);
+        break;
+    case 'h':
+    case 'H': {
+        PrintGestureMenu(params);
+    } break;
+    default: {
+        result = INV_ERROR;
+    } break;
+    };
+    return result;
+}
+
+/**
+ * Initializes the tGestureMenuParams to a set of defaults.
+ *
+ * @param params The parameters to initialize.
+ */
+void GestureMenuSetDefaults(tGestureMenuParams * const params)
+{
+    params->xTapThreshold           = 100;
+    params->yTapThreshold           = 100;
+    params->zTapThreshold           = 100;
+    params->tapTime                 = 100;
+    params->nextTapTime             = 600;
+    params->maxTaps                 = 2;
+    params->shakeRejectValue        = 0.8f;
+    params->xShakeThresh            = 750;
+    params->yShakeThresh            = 750;
+    params->zShakeThresh            = 750;
+    params->xSnapThresh             = 160;
+    params->ySnapThresh             = 320;
+    params->zSnapThresh             = 160;
+    params->shakeTime               = 100;
+    params->nextShakeTime           = 1000;
+    params->shakeFunction           = 0;
+    params->maxShakes               = 3;
+    params->yawRotateTime           = 80;
+    params->yawRotateThreshold      = 70;
+    params->orientationThreshold    = 60;
+    params->sensorsIndex            = 0;
+    params->available_sensors       = INV_NINE_AXIS;
+}
+
+void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+                                    unsigned long available_sensors)
+{
+    params->available_sensors       = available_sensors;
+}
+/**
+ * Call the appropriate MPL set threshold functions and checkes the error codes
+ *
+ * @param params The parametrs to use in setting the thresholds
+ *
+ * @return INV_SUCCESS or the first error code encountered.
+ */
+inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params)
+{
+    inv_error_t result = INV_SUCCESS;
+
+    result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_next_tap_time(params->tapTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+        return result;
+    }
+    result = MLSetNextTapTime(params->nextTapTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_max_taps(params->maxTaps);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_tap_shake_reject(params->shakeRejectValue);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+        return result;
+    }
+
+    //Set up shake gesture
+    result = inv_set_shake_func(params->shakeFunction);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_shake_time(params->shakeTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_next_shake_time(params->nextShakeTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+        return result;
+    }
+
+    // Yaw rotate settings
+    result = inv_set_yaw_rotate_time(params->yawRotateTime);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+        return result;
+    }
+    result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+        return result;
+    }
+
+    // Orientation settings
+    result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80,
+                                       INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result);
+        return result;
+    }
+
+    // Requested Sensors
+    result = inv_set_mpu_sensors(
+        sensors[params->sensorsIndex] & params->available_sensors);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result,
+                 sensors[params->sensorsIndex] & params->available_sensors);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+void PrintGesture(tGesture* gesture)
+{
+    float speed;
+    char type[1024];
+    switch (gesture->type)
+    {
+    case INV_TAP:
+    {
+        if (gesture->meta < 0) {
+            snprintf(type,sizeof(type),"-");
+        } else {
+            snprintf(type,sizeof(type),"+");
+        }
+
+        switch (ABS(gesture->meta))
+        {
+        case 1:
+            strcat(type,"X");
+            break;
+        case 2:
+            strcat(type,"Y");
+            break;
+        case 3:
+            strcat(type,"Z");
+            break;
+        default:
+            strcat(type,"ERROR");
+            break;
+        };
+        MPL_LOGI("TAP: %s  %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n",
+                 type,
+                 gesture->num,
+                 gesture->strength,
+                 gesture->speed,
+                 gesture->reserved,
+                 (180 / M_PI) * atan2(
+                    (float)gesture->strength, (float)gesture->speed),
+                 (180 / M_PI) * atan2(
+                    (float)gesture->speed, (float)gesture->reserved),
+                 (180 / M_PI) * atan2(
+                    (float)gesture->strength, (float)gesture->reserved)
+            );
+    }
+    break;
+    case INV_ROLL_SHAKE:
+    case INV_PITCH_SHAKE:
+    case INV_YAW_SHAKE:
+    {
+        if (gesture->strength){
+            snprintf(type, sizeof(type), "Snap : ");
+        } else {
+            snprintf(type, sizeof(type), "Shake: ");
+        }
+
+        if (gesture->meta==0) {
+            strcat(type, "+");
+        } else {
+            strcat(type, "-");
+        }
+
+        if (gesture->type == INV_ROLL_SHAKE) {
+            strcat(type, "Roll  ");
+        } else if (gesture->type == INV_PITCH_SHAKE) {
+            strcat(type, "Pitch ");
+        } else if (gesture->type == INV_YAW_SHAKE) {
+            strcat(type, "Yaw   ");
+        }
+
+        speed = (float)gesture->speed +
+            (float)(gesture->reserved / (float)(1 << 16));
+        MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed);
+    }
+    break;
+    case INV_YAW_IMAGE_ROTATE:
+    {
+        if (gesture->meta == 0) {
+            snprintf(type, sizeof(type), "Positive ");
+        } else {
+            snprintf(type, sizeof(type), "Negative ");
+        }
+        MPL_LOGI("%s Yaw Image Rotation\n", type);
+    }
+    break;
+    default:
+        MPL_LOGE("Unknown Gesture received\n");
+        break;
+    }
+}
+
+/**
+ * Prints the new or current orientation using MPL_LOGI and remembers the last
+ * orientation to print orientation flips.
+ *
+ * @param orientation the new or current orientation.  0 to reset.
+ */
+void PrintOrientation(unsigned short orientation)
+{
+    // Determine if it was a flip
+    static int sLastOrientation = 0;
+    int flip = orientation | sLastOrientation;
+
+    if ((INV_X_UP | INV_X_DOWN) == flip) {
+        MPL_LOGI("Flip about the X Axis: \n");
+    } else if ((INV_Y_UP | INV_Y_DOWN) == flip) {
+        MPL_LOGI("Flip about the Y axis: \n");
+    } else if ((INV_Z_UP | INV_Z_DOWN) == flip) {
+        MPL_LOGI("Flip about the Z axis: \n");
+    }
+    sLastOrientation = orientation;
+
+    switch (orientation) {
+    case INV_X_UP:
+        MPL_LOGI("X Axis is up\n");
+        break;
+    case INV_X_DOWN:
+        MPL_LOGI("X Axis is down\n");
+        break;
+    case INV_Y_UP:
+        MPL_LOGI("Y Axis is up\n");
+        break;
+    case INV_Y_DOWN:
+        MPL_LOGI("Y Axis is down\n");
+        break;
+    case INV_Z_UP:
+        MPL_LOGI("Z Axis is up\n");
+        break;
+    case INV_Z_DOWN:
+        MPL_LOGI("Z Axis is down\n");
+        break;
+    case 0:
+        break; /* Not an error.  Resets sLastOrientation */
+    default:
+        MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation);
+        break;
+    }
+}
+
+
diff --git a/libsensors/software/simple_apps/common/gestureMenu.h b/libsensors/software/simple_apps/common/gestureMenu.h
new file mode 100644
index 0000000..8f804e1
--- /dev/null
+++ b/libsensors/software/simple_apps/common/gestureMenu.h
@@ -0,0 +1,75 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/***************************************************************************** *
+ * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $ 
+ ******************************************************************************/
+/**
+ * @defgroup 
+ * @brief  
+ *
+ * @{
+ *      @file     gestureMenu.h
+ *      @brief    
+ *
+ *
+ */
+
+#ifndef __GESTUREMENU_H__
+#define __GESTUREMENU_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************/
+    typedef struct sGestureMenuParams {
+        /* Tap Params */
+        int xTapThreshold;
+        int yTapThreshold;
+        int zTapThreshold;
+        int tapTime;
+        int nextTapTime;
+        int maxTaps;
+        float shakeRejectValue;
+
+        /* Shake Params */
+        int xShakeThresh;
+        int yShakeThresh;
+        int zShakeThresh;
+        int xSnapThresh;
+        int ySnapThresh;
+        int zSnapThresh;
+        int shakeTime;
+        int nextShakeTime;
+        int shakeFunction;
+        int maxShakes;
+
+        /* Yaw rotate params */
+        int yawRotateTime;
+        int yawRotateThreshold;
+
+        /* Orientation */
+        float orientationThreshold;
+        int sensorsIndex;
+        unsigned long available_sensors;
+    } tGestureMenuParams;
+
+    void     PrintGestureMenu(tGestureMenuParams const * const params) ;
+    inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch);
+    void     PrintGesture(gesture_t* gesture);
+    void     PrintOrientation(unsigned short orientation);
+    void     GestureMenuSetDefaults(tGestureMenuParams * const params);
+    void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+                                        unsigned long available_sensors);
+    inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params);
+
+/******************************************************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __GESTUREMENU_H__
diff --git a/libsensors/software/simple_apps/common/helper.c b/libsensors/software/simple_apps/common/helper.c
new file mode 100644
index 0000000..4d634bd
--- /dev/null
+++ b/libsensors/software/simple_apps/common/helper.c
@@ -0,0 +1,110 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $
+ *
+ *******************************************************************************/
+
+#include <stdio.h>
+#ifdef _WIN32
+#include <windows.h>
+#include <conio.h>
+#endif
+#ifdef LINUX
+#include <sys/select.h>
+#endif
+#include <time.h>
+#include <string.h>
+
+#include "ml.h"
+#include "slave.h"
+#include "mldl.h"
+#include "mltypes.h"
+#include "mlstates.h"
+#include "compass.h"
+
+#include "mlsl.h"
+#include "ml.h"
+
+#include "helper.h"
+#include "mlsetup.h"
+#include "fopenCMake.h"
+#include "int.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-helper"
+
+#ifdef AIO
+extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode);
+#endif
+
+// Keyboard hit function
+int ConsoleKbhit(void)
+{
+#ifdef _WIN32
+    return _kbhit();
+#else
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0,&read_fd);
+
+    if(select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if(FD_ISSET(0,&read_fd))
+        return 1;
+
+    return 0;
+#endif
+}
+
+char ConsoleGetChar(void) {
+#ifdef _WIN32
+    return _getch();
+#else
+    return getchar();
+#endif
+}
+struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec)
+{
+    struct mpuirq_data **data;
+    void *tmp;
+    int ii;
+    const int irq_data_size = sizeof(**data) * numHandles + 
+        sizeof(*data) * numHandles;
+
+    tmp = (void *)inv_malloc(irq_data_size);
+    memset(tmp, 0, irq_data_size);
+    data = (struct mpuirq_data **)tmp;
+    for (ii = 0; ii < numHandles; ii++) {
+        data[ii] = (struct mpuirq_data *)((unsigned long)tmp +
+            (sizeof(*data) * numHandles) + sizeof(**data) * ii);
+    }
+
+    if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) {
+        for (ii = 0; ii < numHandles; ii++) {
+            if (data[ii]->interruptcount) {
+                inv_interrupt_handler(ii);
+            }
+        }
+    }
+    
+    /* Return data incase the application needs to look at the timestamp or
+       other part of the data */
+    return data;
+}
+
+void InterruptPollDone(struct mpuirq_data ** data)
+{
+    inv_free(data);
+}
diff --git a/libsensors/software/simple_apps/common/helper.h b/libsensors/software/simple_apps/common/helper.h
new file mode 100644
index 0000000..b2da520
--- /dev/null
+++ b/libsensors/software/simple_apps/common/helper.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef HELPER_C_H
+#define HELPER_C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+/*
+    Defines 
+*/
+
+#define CALL_N_CHECK(f) {                                                   \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return r35uLt;                                                      \
+    }                                                                       \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) {                                            \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return;                                                             \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_EXIT(f) {                                              \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        exit (r35uLt);                                                      \
+    }                                                                       \
+}
+
+    
+#define CALL_CHECK_N_CALLBACK(f, cb) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        cb;                                                                 \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) {                                       \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                              \
+        printf("Error in file %s, line %d : %s returned code %s (#%d)\n",   \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        goto label;                                                         \
+    }                                                                       \
+}
+
+#define DEFAULT_PLATFORM        PLATFORM_ID_MSB_V2
+#define DEFAULT_ACCEL_ID        ACCEL_ID_KXTF9
+#define DEFAULT_COMPASS_ID      COMPASS_ID_AK8975
+
+#define DataLogger(x)           NULL
+#define DataLoggerSelector(x)   //
+#define DataLoggerCb(x)         NULL
+#define findComm()              (9)
+#define MenuHwChoice(p,a,c)     (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \
+                                 *c = DEFAULT_COMPASS_ID, INV_ERROR)
+
+    char ConsoleGetChar(void);
+    int ConsoleKbhit(void);
+    struct mpuirq_data **InterruptPoll(
+        int *handles, int numHandles, long tv_sec, long tv_usec);
+    void InterruptPollDone(struct mpuirq_data ** data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // HELPER_C_H
diff --git a/libsensors/software/simple_apps/common/mlerrorcode.c b/libsensors/software/simple_apps/common/mlerrorcode.c
new file mode 100644
index 0000000..25b0df6
--- /dev/null
+++ b/libsensors/software/simple_apps/common/mlerrorcode.c
@@ -0,0 +1,96 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <stdio.h>
+#include <string.h>
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+#define ERROR_CODE_CASE(CODE)   \
+    case CODE:                  \
+        return #CODE            \
+
+/**
+ *  @brief  return a string containing the label assigned to the error code.
+ *  
+ *  @param  errorcode   
+ *              The errorcode value of which the label has to be returned.
+ *
+ *  @return A string containing the error code label.
+ */
+char* MLErrorCode(inv_error_t errorcode) 
+{
+    switch(errorcode) {
+        ERROR_CODE_CASE(INV_SUCCESS);
+        ERROR_CODE_CASE(INV_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED);
+        ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_DMP_STARTED);
+        ERROR_CODE_CASE(INV_ERROR_NOT_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_OPENED);
+        ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED);
+        ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO);
+        ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE);
+        ERROR_CODE_CASE(INV_ERROR_FILE_OPEN);
+        ERROR_CODE_CASE(INV_ERROR_FILE_READ);
+        ERROR_CODE_CASE(INV_ERROR_FILE_WRITE);
+
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_READ);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE);
+        ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+
+        ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION);
+        ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE);
+
+        ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT);
+        ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA);
+        ERROR_CODE_CASE(INV_ERROR_MEMORY_SET);
+
+        ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR);
+        ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR);
+        ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE);
+        ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED);
+        ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED);
+
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY);
+        ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR);
+
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN);
+        ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM);
+
+        default:
+        {
+            #define UNKNOWN_ERROR_CODE 1234
+            return ERROR_NAME(UNKNOWN_ERROR_CODE);
+            break;
+        }
+
+    }
+}
+
+/**
+ *  @}
+ */
diff --git a/libsensors/software/simple_apps/common/mlerrorcode.h b/libsensors/software/simple_apps/common/mlerrorcode.h
new file mode 100644
index 0000000..9a35792
--- /dev/null
+++ b/libsensors/software/simple_apps/common/mlerrorcode.h
@@ -0,0 +1,86 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef _MLERRORCODE_H_
+#define _MLERRORCODE_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Defines 
+*/
+#define CALL_N_CHECK(f) {                                                   \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return r35uLt;                                                      \
+    }                                                                       \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) do {                                         \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        return;                                                             \
+    }                                                                       \
+    } while(0)
+
+#define CALL_CHECK_N_EXIT(f) {                                              \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        exit (r35uLt);                                                      \
+    }                                                                       \
+}
+
+    
+#define CALL_CHECK_N_CALLBACK(f, cb) {                                      \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        cb;                                                                 \
+    }                                                                       \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) {                                       \
+    unsigned int r35uLt = f;                                                \
+    if(INV_SUCCESS != r35uLt) {                                             \
+        MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+                __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt);       \
+        goto label;                                                         \
+    }                                                                       \
+}
+
+char* MLErrorCode(inv_error_t errorcode);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/libsensors/software/simple_apps/common/mlsetup.c b/libsensors/software/simple_apps/common/mlsetup.c
new file mode 100644
index 0000000..f11bce9
--- /dev/null
+++ b/libsensors/software/simple_apps/common/mlsetup.c
@@ -0,0 +1,1722 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $
+ *
+ *****************************************************************************/
+#undef MPL_LOG_NDEBUG
+#ifdef UNITTESTING
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+
+/**
+ *  @defgroup MLSETUP
+ *  @brief  The Motion Library external slaves setup override suite.
+ *
+ *          Use these APIs to override the kernel/default settings in the
+ *          corresponding data structures for gyros, accel, and compass.
+ *
+ *  @{
+ *      @file mlsetup.c
+ *      @brief The Motion Library external slaves setup override suite.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+/*
+    Defines
+*/
+/* these have to appear before inclusion of mpu.h */
+#define CONFIG_MPU_SENSORS_KXSD9         y   // Kionix accel
+#define CONFIG_MPU_SENSORS_KXTF9         y   // Kionix accel
+#define CONFIG_MPU_SENSORS_LIS331DLH     y   // ST accelerometer
+#define CONFIG_MPU_SENSORS_LSM303DLX_A   y   // ST accelerometer in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_LIS3DH        y   // ST accelerometer
+#define CONFIG_MPU_SENSORS_BMA150        y   // Bosch 150 accelerometer
+#define CONFIG_MPU_SENSORS_BMA222        y   // Bosch 222 accelerometer
+#define CONFIG_MPU_SENSORS_BMA250        y   // Bosch 250 accelerometer
+#define CONFIG_MPU_SENSORS_ADXL34X       y   // AD 345 or 346 accelerometer
+#define CONFIG_MPU_SENSORS_MMA8450       y   // Freescale MMA8450 accelerometer
+#define CONFIG_MPU_SENSORS_MMA845X       y   // Freescale MMA845X accelerometer
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y   // Invensense MPU6050 built-in accelerometer
+#endif
+
+#define CONFIG_MPU_SENSORS_AK8975        y   // AKM compass
+#define CONFIG_MPU_SENSORS_AMI30X        y   // AICHI AMI304/305 compass
+#define CONFIG_MPU_SENSORS_AMI306        y   // AICHI AMI306 compass
+#define CONFIG_MPU_SENSORS_HMC5883       y   // Honeywell compass
+#define CONFIG_MPU_SENSORS_LSM303DLX_M   y   // ST compass in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_YAS529        y   // Yamaha compass
+#define CONFIG_MPU_SENSORS_YAS530        y   // Yamaha compass
+#define CONFIG_MPU_SENSORS_MMC314X       y   // MEMSIC compass
+#define CONFIG_MPU_SENSORS_HSCDTD002B    y   // ALPS compass
+#define CONFIG_MPU_SENSORS_HSCDTD004A    y   // ALPS HSCDTD004A compass
+
+#define CONFIG_MPU_SENSORS_BMA085        y   // Bosch 085 pressure
+
+#include "external_hardware.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include "slave.h"
+#include "compass.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mlsetup"
+
+#include "linux/mpu.h"
+
+#include "mlsetup.h"
+
+#ifdef LINUX
+#include "errno.h"
+#endif
+
+/* Override these structures from mldl.c */
+extern struct ext_slave_descr          g_slave_accel;
+extern struct ext_slave_descr          g_slave_compass;
+//extern struct ext_slave_descr          g_slave_pressure;
+/* Platform Data */
+//extern struct mpu_platform_data        g_pdata;
+extern struct ext_slave_platform_data  g_pdata_slave_accel;
+extern struct ext_slave_platform_data  g_pdata_slave_compass;
+//extern struct ext_slave_platform_data  g_pdata_slave_pressure;
+signed char g_gyro_orientation[9];
+
+/*
+    Typedefs
+*/
+typedef void tSetupFuncAccel(void);
+typedef void tSetupFuncCompass(void);
+typedef void tSetupFuncPressure(void);
+
+#ifdef LINUX
+#include <sys/ioctl.h>
+#endif
+
+/*********************************************************************
+              Dragon - PLATFORM_ID_DRAGON_PROTOTYPE
+*********************************************************************/
+/**
+ * @internal
+ * @brief  performs a 180' rotation around Z axis to reflect
+ *         usage of the multi sensor board (MSB) with the
+ *         beagleboard
+ * @note   assumes well formed mounting matrix, with only
+ *         one 1 for each row.
+ */
+static void Rotate180DegAroundZAxis(signed char matrix[])
+{
+    int ii;
+    for(ii=0; ii<6; ii++) {
+        matrix[ii] = -matrix[ii];
+    }
+}
+
+/**
+ * @internal
+ * Sets the orientation based on the position of the mounting.  For different
+ * devices the relative position to pin 1 will be different.
+ *
+ * Positions are:
+ * - 0-3 are Z up
+ * - 4-7 are Z down
+ * - 8-11 are Z right
+ * - 12-15 are Z left
+ * - 16-19 are Z front
+ * - 20-23 are Z back
+ *
+ * @param position The position of the orientation
+ * @param orientation the location to store the new oreintation
+ */
+static inv_error_t SetupOrientation(unsigned int position,
+                                 signed char *orientation)
+{
+    memset(orientation, 0, 9);
+    switch (position){
+    case 0:
+        /*-------------------------*/
+        orientation[0] = +1;
+        orientation[4] =     +1;
+        orientation[8] =         +1;
+        break;
+    case 1:
+        /*-------------------------*/
+        orientation[1] =     +1;
+        orientation[3] = -1;
+        orientation[8] =         +1;
+        break;
+    case 2:
+        /*-------------------------*/
+        orientation[0] = -1;
+        orientation[4] =     -1;
+        orientation[8] =         +1;
+        break;
+    case 3:
+        /*-------------------------*/
+        orientation[1] =     -1;
+        orientation[3] = +1;
+        orientation[8] =         +1;
+        break;
+    case 4:
+        /*-------------------------*/
+        orientation[0] = -1;
+        orientation[4] =     +1;
+        orientation[8] =         -1;
+        break;
+    case 5:
+        /*-------------------------*/
+        orientation[1] =     -1;
+        orientation[3] = -1;
+        orientation[8] =         -1;
+        break;
+    case 6:
+        /*-------------------------*/
+        orientation[0] = +1;
+        orientation[4] =     -1;
+        orientation[8] =         -1;
+        break;
+    case 7:
+        /*-------------------------*/
+        orientation[1] =     +1;
+        orientation[3] = +1;
+        orientation[8] =         -1;
+        break;
+    case 8:
+        /*-------------------------*/
+        orientation[2] =         +1;
+        orientation[3] = +1;
+        orientation[7] =     +1;
+        break;
+    case 9:
+        /*-------------------------*/
+        orientation[2] = +1;
+        orientation[4] = +1;
+        orientation[6] = -1;
+        break;
+    case 10:
+        orientation[2] = +1;
+        orientation[3] = -1;
+        orientation[7] = -1;
+        break;
+    case 11:
+        orientation[2] = +1;
+        orientation[4] = -1;
+        orientation[6] = +1;
+        break;
+    case 12:
+        orientation[2] = -1;
+        orientation[3] = -1;
+        orientation[7] = +1;
+        break;
+    case 13:
+        orientation[2] = -1;
+        orientation[4] = -1;
+        orientation[6] = -1;
+        break;
+    case 14:
+        orientation[2] = -1;
+        orientation[3] = +1;
+        orientation[7] = -1;
+        break;
+    case 15:
+        orientation[2] = -1;
+        orientation[4] = +1;
+        orientation[6] = +1;
+        break;
+    case 16:
+        orientation[0] = -1;
+        orientation[5] = +1;
+        orientation[7] = +1;
+        break;
+    case 17:
+        orientation[1] = -1;
+        orientation[5] = +1;
+        orientation[6] = -1;
+        break;
+    case 18:
+        orientation[0] = +1;
+        orientation[5] = -1;
+        orientation[7] = -1;
+        break;
+    case 19:
+        orientation[1] = -1;
+        orientation[5] = +1;
+        orientation[6] = +1;
+        break;
+    case 20:
+        orientation[0] = +1;
+        orientation[5] = -1;
+        orientation[7] = +1;
+        break;
+    case 21:
+        orientation[1] = -1;
+        orientation[5] = -1;
+        orientation[6] = +1;
+        break;
+    case 22:
+        orientation[0] = -1;
+        orientation[5] = -1;
+        orientation[7] = -1;
+        break;
+    case 23:
+        orientation[1] = +1;
+        orientation[5] = -1;
+        orientation[6] = -1;
+        break;
+    default:
+        MPL_LOGE("Invalid position %d\n", position);
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    return INV_SUCCESS;
+}
+
+static void PrintMountingOrientation(
+    const char * header, signed char *orientation)
+{
+    MPL_LOGV("%s:\n", header);
+    MPL_LOGV("\t[[%3d, %3d, %3d]\n",
+             orientation[0], orientation[1], orientation[2]);
+    MPL_LOGV("\t [%3d, %3d, %3d]\n",
+             orientation[3], orientation[4], orientation[5]);
+    MPL_LOGV("\t [%3d, %3d, %3d]]\n",
+             orientation[6], orientation[7], orientation[8]);
+}
+
+/*****************************
+ *   Accel Setup Functions   *
+ *****************************/
+
+static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 5;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lis331_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS331;
+    return INV_SUCCESS;
+}
+
+static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 1;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 3;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lis3dh_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LIS3DH;
+    return result;
+}
+
+static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 7;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *kxsd9_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXSD9;
+    return result;
+}
+
+static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB_EVB:
+        position =0;
+            break;
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 7;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 1;
+        break;
+#endif
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *kxtf9_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_KXTF9;
+    return result;
+}
+
+static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+        position = 3;
+        break;
+    case PLATFORM_ID_MSB_V2:
+        position = 1;
+        break;
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *lsm303dlx_a_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_LSM303;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 3;
+        break;
+#endif
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma150_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA150;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma222_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA222;
+    return result;
+}
+
+static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 0;
+        break;
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 3;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *bma250_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_BMA250;
+    return result;
+}
+
+static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *adxl34x_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_ADXL34X;
+    return result;
+}
+
+
+static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 5;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *mma8450_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA8450;
+    return result;
+}
+
+
+static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 5;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_accel = *mma845x_get_slave_descr();
+#endif
+    g_pdata_slave_accel.address         = ACCEL_SLAVEADDR_MMA845X;
+    return result;
+}
+
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+	case PLATFORM_ID_DRAGON_USB_DONGLE:
+        position = 1;
+        break;
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    case PLATFORM_ID_MANTIS_EVB:
+        position = 0;
+        break;
+    case PLATFORM_ID_MSB_V3:
+        position = 2;
+        break;
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        return INV_ERROR_INVALID_PARAMETER;
+    };
+
+    SetupOrientation(position, g_pdata_slave_accel.orientation);
+    /* Interrupt */
+#ifndef LINUX
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+   // g_slave_accel = // fixme *mpu6050_get_slave_descr();
+#endif
+#endif
+    g_pdata_slave_accel.address         = 0x68;
+    return result;
+}
+
+/*****************************
+    Compass Setup Functions
+******************************/
+static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+        position = 2;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+        position = 4;
+        break;
+#endif
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+        position = 7;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V3:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DRAGON_USB_DONGLE:
+    case PLATFORM_ID_MSB_EVB:
+        position = 5;
+        break;
+    case PLATFORM_ID_MANTIS_EVB:
+        position = 4;
+        break;
+	 case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *ak8975_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AKM;
+    return result;
+}
+
+static inv_error_t SetupCompassMMCCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 7;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *mmc314x_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_MMC314X;
+    return result;
+}
+
+static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 4;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI304;
+#ifndef LINUX
+    g_slave_compass = *ami30x_get_slave_descr();
+#endif
+    return result;
+}
+
+static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 3;
+        break;
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *ami306_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_AMI306;
+    return result;
+}
+
+static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+#ifdef WIN32
+    case PLATFORM_ID_DONGLE:
+        position = 2;
+        break;
+#endif
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hmc5883_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+
+static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+#ifndef LINUX
+    g_slave_compass = *lsm303dlx_m_get_slave_descr();
+    g_slave_compass.id = COMPASS_ID_LSM303DLH;
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+        position = 8;
+        break;
+    case PLATFORM_ID_MSB_V2:
+        position = 12;
+        break;
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *lsm303dlx_m_get_slave_descr();
+    g_slave_compass.id = COMPASS_ID_LSM303DLM;
+#endif
+    g_pdata_slave_compass.address       = COMPASS_SLAVEADDR_HMC5883;
+    return result;
+}
+
+static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *yas530_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS530;
+    return result;
+}
+
+static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 6;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *yas529_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_YAS529;
+    return result;
+}
+
+
+static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 2;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hscdtd002b_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
+    return result;
+}
+
+static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+        position = 1;
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_MANTIS_MSB:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    };
+    result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+#ifndef LINUX
+    g_slave_compass = *hscdtd004a_get_slave_descr();
+#endif
+    g_pdata_slave_compass.address         = COMPASS_SLAVEADDR_HSCDTD00XX;
+    return result;
+}
+
+
+/*****************************
+    Pressure Setup Functions
+******************************/
+#if 0
+static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId)
+{
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation));
+
+    g_pdata_slave_pressure.bus             = EXT_SLAVE_BUS_PRIMARY;
+#ifndef LINUX
+    g_slave_pressure = *bma085_get_slave_descr();
+#endif
+    g_pdata_slave_pressure.address         = PRESSURE_SLAVEADDR_BMA085;
+    return INV_SUCCESS;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelCalibration(unsigned short platformId,
+                                      unsigned short accelId)
+{
+    /*----  setup the accels ----*/
+    switch(accelId) {
+    case ACCEL_ID_LSM303DLX:
+        SetupAccelLSM303Calibration(platformId);
+        break;
+    case ACCEL_ID_LIS331:
+        SetupAccelSTLIS331Calibration(platformId);
+        break;
+    case ACCEL_ID_KXSD9:
+        SetupAccelKionixKXSD9Calibration(platformId);
+        break;
+    case ACCEL_ID_KXTF9:
+        SetupAccelKionixKXTF9Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA150:
+        SetupAccelBMA150Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA222:
+        SetupAccelBMA222Calibration(platformId);
+        break;
+    case ACCEL_ID_BMA250:
+        SetupAccelBMA250Calibration(platformId);
+        break;
+    case ACCEL_ID_ADXL34X:
+        SetupAccelADXL34XCalibration(platformId);
+        break;
+    case ACCEL_ID_MMA8450:
+        SetupAccelMMA8450Calibration(platformId);
+        break;
+    case ACCEL_ID_MMA845X:
+        SetupAccelMMA845XCalibration(platformId);
+        break;
+    case ACCEL_ID_LIS3DH:
+        SetupAccelSTLIS3DHCalibration(platformId);
+        break;
+    case ACCEL_ID_MPU6050:
+        SetupAccelMPU6050Calibration(platformId);
+        break;
+    case ID_INVALID:
+        break;
+    default:
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) {
+        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_SECONDARY;
+    } else if (accelId != ACCEL_ID_MPU6050) {
+        g_pdata_slave_accel.bus             = EXT_SLAVE_BUS_PRIMARY;
+    }
+
+#ifndef WIN32
+    if (accelId != ID_INVALID)
+        Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation);
+#endif
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t SetupCompassCalibration(unsigned short platformId,
+                                 unsigned short compassId)
+{
+    /*----  setup the compass ----*/
+    switch(compassId) {
+    case COMPASS_ID_AK8975:
+        SetupCompassAKM8975Calibration(platformId);
+        break;
+    case COMPASS_ID_AMI30X:
+        SetupCompassAMI304Calibration(platformId);
+        break;
+    case COMPASS_ID_AMI306:
+        SetupCompassAMI306Calibration(platformId);
+        break;
+    case COMPASS_ID_LSM303DLH:
+        SetupCompassLSM303DLHCalibration(platformId);
+        break;
+    case COMPASS_ID_LSM303DLM:
+        SetupCompassLSM303DLMCalibration(platformId);
+        break;
+    case COMPASS_ID_HMC5883:
+        SetupCompassHMC5883Calibration(platformId);
+        break;
+    case COMPASS_ID_YAS529:
+        SetupCompassYAS529Calibration(platformId);
+        break;
+    case COMPASS_ID_YAS530:
+        SetupCompassYAS530Calibration(platformId);
+        break;
+    case COMPASS_ID_MMC314X:
+        SetupCompassMMCCalibration(platformId);
+        break;
+    case COMPASS_ID_HSCDTD002B:
+        SetupCompassHSCDTD002BCalibration(platformId);
+        break;
+    case COMPASS_ID_HSCDTD004A:
+        SetupCompassHSCDTD004ACalibration(platformId);
+        break;
+    case ID_INVALID:
+        break;
+    default:
+        if (INV_ERROR_INVALID_PARAMETER) {
+            LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+        break;
+    }
+
+    if (platformId == PLATFORM_ID_MSB_V2_MANTIS     ||
+        platformId == PLATFORM_ID_MANTIS_MSB        ||
+        platformId == PLATFORM_ID_MANTIS_USB_DONGLE ||
+        platformId == PLATFORM_ID_MANTIS_PROTOTYPE  ||
+        platformId == PLATFORM_ID_DRAGON_PROTOTYPE) {
+        switch (compassId) {
+        case ID_INVALID:
+            g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID;
+            break;
+        case COMPASS_ID_AK8975:
+        case COMPASS_ID_AMI306:
+            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_SECONDARY;
+            break;
+        default:
+            g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
+        };
+    } else {
+        g_pdata_slave_compass.bus             = EXT_SLAVE_BUS_PRIMARY;
+    }
+
+#ifndef WIN32
+    if (compassId != ID_INVALID)
+        Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation);
+#endif
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+#if 0
+inv_error_t SetupPressureCalibration(unsigned short platformId,
+                                  unsigned short pressureId)
+{
+    inv_error_t result = INV_SUCCESS;
+    /*----  setup the compass ----*/
+    switch(pressureId) {
+        case PRESSURE_ID_BMA085:
+            result = SetupPressureBMA085Calibration(platformId);
+            break;
+        default:
+            if (INV_ERROR_INVALID_PARAMETER) {
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+                return INV_ERROR_INVALID_PARAMETER;
+            }
+    };
+
+    return result;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupGyroCalibration(unsigned short platformId)
+{
+    int position;
+    MPL_LOGV("Calibrating '%s'\n", __func__);
+
+    /* Orientation */
+    switch (platformId) {
+    case PLATFORM_ID_SPIDER_PROTOTYPE:
+        position = 2;
+        break;
+    case PLATFORM_ID_MSB:
+    case PLATFORM_ID_MSB_10AXIS:
+    case PLATFORM_ID_MANTIS_MSB:
+#ifndef WIN32
+        position = 4;
+#else
+        position = 6;
+#endif
+        break;
+    case PLATFORM_ID_DONGLE:
+    case PLATFORM_ID_MANTIS_USB_DONGLE:
+        position = 1;
+        break;
+    case PLATFORM_ID_DRAGON_USB_DONGLE:
+        position = 3;
+        break;
+    case PLATFORM_ID_MANTIS_PROTOTYPE:
+    case PLATFORM_ID_DRAGON_PROTOTYPE:
+    case PLATFORM_ID_ST_6AXIS:
+    case PLATFORM_ID_MSB_V2:
+    case PLATFORM_ID_MSB_V2_MANTIS:
+#ifndef WIN32
+        position = 2;
+#else
+        position = 0;
+#endif
+        break;
+    case PLATFORM_ID_MANTIS_EVB:
+    case PLATFORM_ID_MSB_EVB:
+        position = 0;
+        break;
+    case PLATFORM_ID_MSB_V3:
+        position = 2;
+        break;
+    default:
+        MPL_LOGE("Unsupported platform %d\n", platformId);
+        return INV_ERROR_INVALID_PARAMETER;
+    };
+
+    SetupOrientation(position, g_gyro_orientation);
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Setup the Hw orientation and full scale.
+ *  @param  platfromId
+ *              an user defined Id to distinguish the Hw platform in
+ *              use from others.
+ *  @param  accelId
+ *              the accelerometer specific id, as specified in the MPL.
+ *  @param  compassId
+ *              the compass specific id, as specified in the MPL.
+ *  @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t SetupPlatform(
+                unsigned short platformId,
+                unsigned short accelId,
+                unsigned short compassId)
+{
+    int result;
+
+    memset(&g_slave_accel, 0, sizeof(g_slave_accel));
+    memset(&g_slave_compass, 0, sizeof(g_slave_compass));
+//    memset(&g_slave_pressure, 0, sizeof(g_slave_pressure));
+//    memset(&g_pdata, 0, sizeof(g_pdata));
+
+#ifdef LINUX
+    /* On Linux initialize the global platform data with the driver defaults */
+    {
+        void *mpu_handle;
+        int ii;
+
+        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+        MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n");
+        result = inv_serial_open("/dev/mpu",&mpu_handle);
+        if (result) {
+            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!slave[ii])
+                continue;
+            slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR,
+                           slave[ii]);
+            if (result)
+                result = errno;
+            if(result == INV_ERROR_INVALID_MODULE) {
+                slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE);
+                return result;
+            }
+        }
+        //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata);
+        if (result) {
+            result = errno;
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!pdata_slave[ii])
+                continue;
+            pdata_slave[ii]->type = ii;
+            result = ioctl(
+                (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA,
+                pdata_slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                pdata_slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        if (result) {
+            MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+        }
+        inv_serial_close(mpu_handle);
+    }
+#endif
+
+    result = SetupGyroCalibration(platformId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Gyroscope", g_gyro_orientation);
+    result = SetupAccelCalibration(platformId, accelId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation);
+    result = SetupCompassCalibration(platformId, compassId);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation);
+#if 0
+    if (platformId == PLATFORM_ID_MSB_10AXIS) {
+        result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation);
+    }
+#endif
+#ifdef LINUX
+    /* On Linux override the orientation, level shifter etc */
+    {
+        void *mpu_handle;
+        int ii;
+        struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+        struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+        slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+        slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+        //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+        pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+        pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+        pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+        //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+        MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n");
+        result = inv_serial_open("/dev/mpu",&mpu_handle);
+        if (result) {
+            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!slave[ii])
+                continue;
+            slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+                           slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata);
+        if (result) {
+            result = errno;
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+            if (!pdata_slave[ii])
+                continue;
+            pdata_slave[ii]->type = ii;
+            result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+                pdata_slave[ii]);
+            if (result)
+                result = errno;
+            if (result == INV_ERROR_INVALID_MODULE) {
+                pdata_slave[ii] = NULL;
+                result = 0;
+            } else if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+        if (result) {
+            MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+        }
+        inv_serial_close(mpu_handle);
+    }
+#endif
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
+
+
diff --git a/libsensors/software/simple_apps/common/mlsetup.h b/libsensors/software/simple_apps/common/mlsetup.h
new file mode 100644
index 0000000..06fa9f4
--- /dev/null
+++ b/libsensors/software/simple_apps/common/mlsetup.h
@@ -0,0 +1,52 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+ 
+/*******************************************************************************
+ *
+ * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $
+ *
+ *******************************************************************************/
+
+#ifndef MLSETUP_H
+#define MLSETUP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "linux/mpu.h"
+#include "mltypes.h"
+
+    enum mpu_platform_id {
+        PLATFORM_ID_INVALID = ID_INVALID, // 0
+        PLATFORM_ID_MSB,              // (0x0001) MSB (Multi sensors board)
+        PLATFORM_ID_ST_6AXIS,         // (0x0002) 6 Axis with ST accelerometer
+        PLATFORM_ID_DONGLE,           // (0x0003) 9 Axis USB dongle with
+        PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board
+        PLATFORM_ID_MANTIS_MSB,       // (0x0005) MSB with Mantis
+        PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle.
+        PLATFORM_ID_MSB_10AXIS,       // (0x0007) MSB with pressure sensor
+        PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board
+        PLATFORM_ID_MSB_V2,           // (0x0009) Version 2 MSB
+        PLATFORM_ID_MSB_V2_MANTIS,    // (0x000A) Version 2 MSB with mantis
+        PLATFORM_ID_MANTIS_EVB,       // (0x000B) Mantis EVB (shipped to cust.)
+        PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C
+        PLATFORM_ID_MSB_EVB,          // (0X000D) MSB with 3050. 
+        PLATFORM_ID_SPIDER_PROTOTYPE,
+        PLATFORM_ID_MSB_V3,
+
+        NUM_PLATFORM_IDS
+    };
+    // Main entry APIs
+inv_error_t SetupPlatform(unsigned short platformId, 
+                       unsigned short accelSelection, 
+                       unsigned short compassSelection);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MLSETUP_H */
diff --git a/mlsdk/mlutils/slave.h b/libsensors/software/simple_apps/common/slave.h
index 45449f6..7b40a8c 100644
--- a/mlsdk/mlutils/slave.h
+++ b/libsensors/software/simple_apps/common/slave.h
@@ -1,23 +1,11 @@
 /*
  $License:
-   Copyright 2011 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.
-  $
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
  */
 /*******************************************************************************
  *
- * $Id: slave.h 5629 2011-06-11 03:13:08Z mcaramello $
+ * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $
  *
  *******************************************************************************/
 
@@ -34,7 +22,7 @@
  */
 
 #include "mltypes.h"
-#include "mpu.h"
+#include "linux/mpu.h"
 
     /* ------------ */
     /* - Defines. - */
@@ -72,7 +60,7 @@
 
 #if ACCEL_ST_LSM303
 #define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_LSM303
-#define DEFAULT_ACCEL_ID                  ACCEL_ID_LSM303A
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_LSM303DLX
 #endif
 
 #if ACCEL_KIONIX_KXSD9
diff --git a/libsensors/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors/software/simple_apps/console/linux/build/android/consoledmp-shared
new file mode 100644
index 0000000..228abf7
--- /dev/null
+++ b/libsensors/software/simple_apps/console/linux/build/android/consoledmp-shared
diff --git a/libsensors/software/simple_apps/console/linux/build/android/shared.mk b/libsensors/software/simple_apps/console/linux/build/android/shared.mk
new file mode 100644
index 0000000..b1d881c
--- /dev/null
+++ b/libsensors/software/simple_apps/console/linux/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = consoledmp$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors/software/simple_apps/console/linux/build/filelist.mk b/libsensors/software/simple_apps/console/linux/build/filelist.mk
new file mode 100644
index 0000000..b01fdfb
--- /dev/null
+++ b/libsensors/software/simple_apps/console/linux/build/filelist.mk
@@ -0,0 +1,23 @@
+#### filelist.mk for console_test ####
+
+# helper headers
+HEADERS := $(COMMON_DIR)/external_hardware.h
+HEADERS += $(COMMON_DIR)/fopenCMake.h
+HEADERS += $(COMMON_DIR)/helper.h
+HEADERS += $(COMMON_DIR)/mlerrorcode.h
+HEADERS += $(COMMON_DIR)/mlsetup.h
+HEADERS += $(COMMON_DIR)/slave.h
+
+HEADERS += $(HAL_DIR)/include/mlos.h
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/console_test.c
+
+# helper sources
+SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c
+SOURCES += $(HAL_DIR)/linux/mlos_linux.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors/software/simple_apps/console/linux/console_test.c b/libsensors/software/simple_apps/console/linux/console_test.c
new file mode 100644
index 0000000..e15b20d
--- /dev/null
+++ b/libsensors/software/simple_apps/console/linux/console_test.c
@@ -0,0 +1,742 @@
+/*******************************************************************************
+ * $Id: $
+ ******************************************************************************/
+ 
+/*******************************************************************************
+ *
+ * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
+ *
+ ******************************************************************************/
+//#include <linux/conio.h>
+//#include <fcntl.h>
+//#include <termios.h>
+//#include <unistd.h>
+
+//#if 0
+#include <stdio.h>
+#include <time.h>
+#include <sys/select.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <linux/input.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "console_test"
+
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "inv_sysfs_utils.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+/*#else
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include "inv_sysfs_utils.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+#include "log.h"
+#endif*/
+
+/* TODO: add devices as needed. */
+#define ITG3500         (0)
+#define MPU6050         (1)
+#define BMA250          (2)
+#define NUM_DEVICES     (ITG3500 + MPU6050 + BMA250)
+
+#define DEVICE          MPU6050
+#define DMP_IMAGE       dmp_firmware_200_latest
+#define SIX_AXES        6
+#define NINE_AXES       9
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#endif
+
+/* TODO: Add paths to other attributes.
+ * TODO: Input device paths depend on the module installation order.
+ */
+const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
+    {        /* ITG3500 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
+        .raw_data           = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
+        .temperature        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
+        .fifo_rate          = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
+        .power_state        = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
+        .fsr                = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
+        .lpf                = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
+        .scale              = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
+        .temp_scale         = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
+        .temp_offset        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
+        .self_test          = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    },
+
+    {        /* MPU6050 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/class/invensense/mpu/enable",
+        .raw_data           = "/sys/class/invensense/mpu/raw_gyro",
+        .temperature        = "/sys/class/invensense/mpu/temperature",
+        .fifo_rate          = "/sys/class/invensense/mpu/fifo_rate",
+        .power_state        = "/sys/class/invensense/mpu/power_state",
+        .fsr                = "/sys/class/invensense/mpu/FSR",
+        .lpf                = "/sys/class/invensense/mpu/lpf",
+        .scale              = "/sys/class/invensense/mpu/gyro_scale",
+        .temp_scale         = "/sys/class/invensense/mpu/temp_scale",
+        .temp_offset        = "/sys/class/invensense/mpu/temp_offset",
+        .self_test          = "/sys/class/invensense/mpu/self_test",
+        .accel_en           = "/sys/class/invensense/mpu/accl_enable",
+        .accel_fifo_en      = "/sys/class/invensense/mpu/accl_fifo_enable",
+        .accel_fs           = "/sys/class/invensense/mpu/accl_fs",
+        .clock_source       = "/sys/class/invensense/mpu/clock_source",
+        .early_suspend_en   = "/sys/class/invensense/mpu/early_suspend_enable",
+           .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
+        .gyro_en            = "/sys/class/invensense/mpu/gyro_enable",
+        .gyro_fifo_en       = "/sys/class/invensense/mpu/gyro_fifo_enable",
+        .key                = "/sys/class/invensense/mpu/key",
+        .raw_accel          = "/sys/class/invensense/mpu/raw_accl",
+        .reg_dump           = "/sys/class/invensense/mpu/reg_dump",
+        .tap_on             = "/sys/class/invensense/mpu/tap_on",
+         .dmp_firmware      = "/sys/class/invensense/mpu/dmp_firmware"
+    },
+
+    {        /* BMA250 */
+        .buffer             = "/dev/input/input/event1",
+        .enable             = "/sys/devices/virtual/input/input1/enable",
+        .raw_data           = "/sys/devices/virtual/input/input1/value",
+        .temperature        = NULL,
+        .fifo_rate          = NULL,
+        .power_state        = NULL,
+        .fsr                = NULL,
+        .lpf                = NULL,
+        .scale              = NULL,
+        .temp_scale         = NULL,
+        .temp_offset        = NULL,
+        .self_test          = NULL,
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    }
+};
+
+static void (*s_func_cb) (void);
+
+int inv_read_data(char *names, char *data)
+{
+    char str[8];
+    int count;
+    short s_data;
+
+    count = inv_sysfs_read((char*)names, sizeof(str), str);
+    if (count < 0)
+        return count;
+    count = sscanf(str, "%hd", &s_data);
+    *data = s_data;
+    if (count < 1)
+        return -EAGAIN;
+    return count;
+
+}
+
+void fifoCB(void)
+{
+    if (1) {
+        float gyro[3];
+        float accel[3];
+        float orient[3];
+        float rv[3];
+
+        int8_t accuracy;
+        inv_time_t timestamp;
+
+        printf("/*************************************************\n"); 
+        inv_get_sensor_type_gyroscope(gyro, &accuracy, &timestamp);
+        printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     gyro[0],
+                     gyro[1],
+                     gyro[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_accelerometer(accel, &accuracy, &timestamp);
+        printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     accel[0],
+                     accel[1],
+                     accel[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_rotation_vector(rv, &accuracy, &timestamp);
+        printf("RV %7.3f %7.3f %7.3f %5d %9lld\n",
+                     rv[0],rv[1],rv[2],accuracy,timestamp);
+
+        inv_get_sensor_type_orientation(orient, &accuracy, &timestamp);
+        printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n",
+                     orient[0],orient[1],orient[2],accuracy,timestamp);
+        printf("/*************************************************\n"); 
+    }
+}
+
+unsigned short orient;
+signed char g_gyro_orientation[9] = {1, 0, 0,
+                                     0, 1, 0,
+                                     0, 0, 1};
+
+signed char g_accel_orientation[9] = {-1, 0, 0,
+                                      0, -1, 0,
+                                      0, 0, 1};
+float scale;
+float range;
+long sens;
+
+
+
+short mTempOffset = 0;
+short mTempScale = 0;
+bool mFirstRead = 1;
+
+/******************* FUNCTIONS *******************************/
+#if 0
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;                  // error
+    return b;
+}
+#endif 
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+    s_func_cb = func_cb;
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief Keyboard hit function.
+ */
+int kbhit(void)
+{
+#if 1
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0,&read_fd);
+
+    if (select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if (FD_ISSET(0,&read_fd))
+        return 1;
+
+    return 0;
+#else
+    struct timeval tv;  
+    fd_set rdfs;   
+    
+    tv.tv_sec = 0;  
+    tv.tv_usec = 0;   
+    
+    FD_ZERO(&rdfs);  
+    FD_SET (STDIN_FILENO, &rdfs);   
+    
+    select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);  
+    return FD_ISSET(STDIN_FILENO, &rdfs); 
+#endif
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+    inv_error_t result;
+  
+    result = inv_enable_quaternion();
+    if (result) {
+        if (result == INV_ERROR_NOT_AUTHORIZED) {
+            LOGE("Enable Quaternion failed: not authorized");
+        }
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_motion_no_motion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_gyro_tc();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_hal_outputs();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+int read_attribute_sensor(int fd, char *data, unsigned int size)
+{
+    int count = 0;
+        if (fd >=0) {
+            count = read(fd, data, size);
+            if(count < 0) {
+                MPL_LOGE("read fails with error code=%d", count);
+            }
+            close(fd);
+        }
+        return count;
+}
+
+int inv_read_temperature(long long *data)
+{
+        int count = 0;
+        int fd;
+
+        if(mFirstRead) {
+            char buf[4];
+            fd = open(filenames[ITG3500].temp_scale, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempscale");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_scale");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempScale);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp scale = %d", mTempScale);
+ 
+            fd = open(filenames[ITG3500].temp_offset, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempoffset");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_offset");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempOffset);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp offset = %d", mTempOffset);
+
+            mFirstRead = false;
+        }
+
+        char raw_buf[25];
+        short raw;
+        long long timestamp;
+        fd = open(filenames[ITG3500].temperature, O_RDONLY);
+        if(fd < 0) {
+            MPL_LOGE("errors opening temperature");
+            return -1;
+        }
+
+        memset(raw_buf, 0, sizeof(raw_buf));
+
+        count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+        if(count < 0) {
+            MPL_LOGE("errors reading temperature");
+            return -1;
+        }
+        count = sscanf(raw_buf, "%hd%lld", &raw, &timestamp);
+        if(count < -1)
+            return -1;
+        MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp);
+        MPL_LOGI("temperature offset = %d", mTempOffset);
+        MPL_LOGI("temperature scale = %d", mTempScale);
+        int adjuster = 35 + ((raw-mTempOffset)/mTempScale);
+        MPL_LOGI("pre-scaled temperature = %d", adjuster);
+        MPL_LOGI("adjusted temperature = %d", adjuster*65536);
+        //data[0] = adjuster * 65536;
+        data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f;
+        data[1] = timestamp;
+        return 0;
+}
+
+int self_test(void)
+{
+        int err = 0;
+        char str[50];
+        char x[9], y[9], z[9];
+        char pass[2];
+        int fd;
+
+        fd = open((char*)filenames[DEVICE].self_test, O_RDONLY);
+        if(fd < 0) {
+            return fd;
+        }
+        memset(str, 0, sizeof(str));
+        err = read_attribute_sensor(fd, str, sizeof(str));
+        if(err < 0) {
+            return err;
+        }
+        MPL_LOGI("self_test result: %s", str);
+        printf("Self test result: %s ", str);
+        err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass);
+        if(err < 1) {
+            return err;
+        }
+        MPL_LOGI("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        //printf("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        if (atoi(pass)) {
+            MPL_LOGI("Result : PASS (1)");
+            printf("----> PASS (1)\n");
+        } else {
+            MPL_LOGI("Result : FAIL (0)");
+            printf("----> FAIL (0)\n");
+        }
+        return err;
+}
+
+/*******************************************************************************
+ ******************************* MAIN ******************************************
+ ******************************************************************************/
+
+/**
+ * @brief Main function
+ */
+int main(int argc, char *argv[])
+{
+    int key = 0;
+    int ready;
+    long accel[3];
+    short gyro[3];
+    long long timestamp = 0;
+    inv_error_t result;
+
+    char data;
+    unsigned char i;
+    int fd, bytes_read;
+    struct pollfd pfd;
+    unsigned long long time_stamp;
+    unsigned int time_H;
+    struct input_event ev[100];
+#ifdef INV_PLAYBACK_DBG
+    int logging = false;
+    FILE *logfile = NULL;
+#endif
+
+    result = inv_init_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    // Master Enabling.  This also turns on power_state
+    if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0)
+       printf("ERR- Failed to enable event generation\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].enable, &data);
+       printf("Event enable= %d\n", data);
+    }
+
+    // Power ON - No need after master enable above but do it anyway
+    if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0)
+       printf("ERR- Failed to set power state=1\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].power_state, &data);
+       printf("Power state: %d\n", data);
+    }
+    
+    // Turn on tap
+    if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) {
+       printf("ERR- Failed to enable Tap On\n");
+    } 
+    else {
+          inv_read_data((char *)filenames[DEVICE].tap_on, &data);
+       printf("Tap-on: %d\n", data);
+    }
+
+    // Program DMP code.  No longer required to enable tap-on first
+    if ((result = 
+            inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) {
+        printf("ERR- Failed to initiate DMP re-programming %d\n",result);
+    } else {
+         if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) {
+            printf("ERR- Failed file open to write DMP\n");
+            close(fd);
+            exit(0);
+        } else {
+            // Program 200Hz version
+            //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE));
+            //printf("Downloaded %d byte(s) to DMP\n", result);
+            result = inv_load_dmp(fd);
+            //LOG_RESULT_LOCATION(result);
+            close(fd);
+        }
+    }
+
+    // Query DMP running.  For now check by 'firmware_loaded' status 
+    if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) {
+        printf("ERR- Failed to read 'firmware_loaded'\n");
+    } else {
+        printf("Firmware Loaded/ DMP running: %d\n", data);
+    }
+
+    inv_set_fifo_processed_callback(fifoCB);
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        printf ("MPL started\n");
+    }
+
+    /* Gyro Setup */
+    orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+    inv_set_gyro_orientation_and_scale(orient,2000L<<15);
+
+    /* Accel Setup */
+    orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+    /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data.
+     * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits.
+     * 2 * 1L<<15 * 1<<6 ==  1LL<<22
+     */
+    inv_set_accel_orientation_and_scale(orient, 1LL<<22); 
+
+    // Enable Gyro
+    if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0)
+        printf("ERR- Failed to enable Gyro\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].gyro_en, &data);
+        printf("Gyro enable: %d\n", data);
+    }
+
+    // Enable Accel
+    if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0)
+        printf("ERR- Failed to enable Accel\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].accel_en, &data);
+        printf("Accel enable: %d\n", data);
+    }
+
+    // polling for data
+    fd = open(filenames[DEVICE].buffer, O_RDONLY);
+    if(fd < 0) {
+         MPL_LOGE("Cannot open device event buffer");
+    }
+    
+    pfd.fd = fd;
+    pfd.events = POLLIN;
+
+    while (1) {
+
+       result = kbhit();
+        if (result) {
+           key = getchar();
+        } else {
+           key = 0;
+        }
+        if (key == 'l') {
+           MPL_LOGI(" 'l' - load calibration file");
+           inv_load_calibration();
+        }
+        if (key == 't') {
+           MPL_LOGI(" 't' - self test");
+           self_test();
+        }
+        if (key == 'q') {
+            MPL_LOGI(" 'q' - store calibration file");
+            inv_store_calibration();
+            break;
+        }
+#ifdef INV_PLAYBACK_DBG
+        if (key == 's') {
+            if (!logging) {
+                MPL_LOGI(" 's' - toggle logging on");
+                logfile = fopen("/data/playback.bin", "wb");
+                if (logfile) {
+                    inv_turn_on_data_logging(logfile);
+                    logging = true;
+                } else {
+                    MPL_LOGI("Error : "
+                             "cannot open log file '/data/playback.bin'");
+                }
+            } else {
+                MPL_LOGI(" 's' - toggle logging off");
+                inv_turn_off_data_logging();
+                fclose(logfile);
+                logging = false;
+            }
+            break;
+        }
+#endif
+
+        ready = poll(&pfd, 1, 100);
+        if (ready) {
+            bytes_read = read_attribute_sensor(fd, (char *)ev,
+                                      sizeof(struct input_event) * SIX_AXES);
+            //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES);
+            if (bytes_read > 0) {
+                int executed;
+
+                for (i = 0; i < bytes_read / sizeof(struct input_event); i++) {
+                    if (ev[i].type == EV_REL) {
+                        switch (ev[i].code) {
+                        case REL_X:
+                            printf("REL_X\n");
+                            gyro[0]= ev[i].value;                    //Gyro X
+                            printf("Gyro X:%5d ", gyro[0]);        
+                            break;
+                        case REL_Y:
+                            printf("REL_Y\n");
+                            gyro[1]= ev[i].value;                    //Gyro Y
+                            printf("Gyro Y:%5d ", gyro[1]);    
+                            break;
+                        case REL_Z:
+                            printf("REL_Z\n");
+                            gyro[2]= ev[i].value;                    //Gyro Z
+                            printf("Gyro Z:%5d ", gyro[2]);        
+                            break;
+                        case REL_RX:
+                            printf("REL_RX\n");
+                            accel[0]= ev[i].value;                   //Accel X
+                            printf("Accl X:%5ld ", accel[0]);
+                            break;
+                        case REL_RY:
+                            printf("REL_RY\n");
+                            accel[1]= ev[i].value;                   //Accel Y
+                            printf("Accl Y:%5ld ", accel[1]);    
+                            break;
+                        case REL_RZ:
+                            printf("REL_RZ\n");
+                            accel[2]= ev[i].value;                   //Accel Z
+                            printf("Accl Z:%5ld ", accel[2]);    
+                            break;
+                        case REL_MISC:
+                            time_H= ev[i].value;
+                            break;
+                        case REL_WHEEL:
+                            time_stamp = ((unsigned long long)(time_H) << 32) + 
+                                           (unsigned int)ev[i].value;
+                            break;
+                        default:
+                            printf("ERR- Un-recognized event code:  %5d ", ev[i].code);
+                            break;
+                        }
+                    } else {
+#if 0
+                        clock_gettime(CLOCK_MONOTONIC, &timer);
+                        curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL;                        
+                        printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL);
+#endif
+                    }
+                }
+
+                // build & process gyro + accel data
+                result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                } else if ((result = inv_build_accel(accel, 0,
+                                                    (inv_time_t)timestamp,
+                                                    &executed))) {
+                    LOG_RESULT_LOCATION(result);
+                } 
+                if (executed) {
+                    printf("Exec on data Ok\n");
+                    s_func_cb();
+                }
+
+           } else {
+               //printf ("ERR- No data!\n");
+           }
+
+        } else { MPL_LOGV("Device not ready"); }
+    }
+    close(fd);
+    
+#ifdef INV_PLAYBACK_DBG
+    if (logging) {
+        inv_turn_off_data_logging();
+        fclose(logfile);
+    }
+#endif
+
+    return 0;
+}
diff --git a/libsensors/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors/software/simple_apps/input_sub/build/android/input_gyro-shared
new file mode 100644
index 0000000..5d52b21
--- /dev/null
+++ b/libsensors/software/simple_apps/input_sub/build/android/input_gyro-shared
diff --git a/libsensors/software/simple_apps/input_sub/build/android/shared.mk b/libsensors/software/simple_apps/input_sub/build/android/shared.mk
new file mode 100644
index 0000000..7f6cc43
--- /dev/null
+++ b/libsensors/software/simple_apps/input_sub/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = input_gyro$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors/software/simple_apps/input_sub/build/filelist.mk b/libsensors/software/simple_apps/input_sub/build/filelist.mk
new file mode 100644
index 0000000..0936212
--- /dev/null
+++ b/libsensors/software/simple_apps/input_sub/build/filelist.mk
@@ -0,0 +1,13 @@
+#### filelist.mk for input_gyro ####
+
+# helper headers
+HEADERS := $(MPL_DIR)/authenticate.h
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/test_input_gyro.c
+
+INV_SOURCES += $(SOURCES)
+
+#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
+VPATH += $(APP_DIR)
diff --git a/libsensors/software/simple_apps/input_sub/test_input_gyro.c b/libsensors/software/simple_apps/input_sub/test_input_gyro.c
new file mode 100644
index 0000000..6fa9aab
--- /dev/null
+++ b/libsensors/software/simple_apps/input_sub/test_input_gyro.c
@@ -0,0 +1,485 @@
+/*
+ * input interface testing
+ */
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/input.h>
+#include <linux/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "linux/ml_sysfs_helper.h"
+#include "authenticate.h"
+#include "ml_load_dmp.h"
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#endif
+
+void HandleOrient(int orient)
+{    
+    if (orient & 0x01)
+        printf("INV_X_UP\n");
+    if (orient & 0x02) 
+        printf("INV_X_DOWN\n");
+    if (orient & 0x04) 
+        printf("INV_Y_UP\n");
+    if (orient & 0x08) 
+        printf("INV_Y_DOWN\n");
+    if (orient & 0x10) 
+        printf("INV_Z_UP\n");
+    if (orient & 0x20) 
+        printf("INV_Z_DOWN\n");
+    if (orient & 0x40) 
+        printf("INV_ORIENTATION_FLIP\n");
+}
+
+void HandleTap(int tap)
+{
+    int tap_dir = tap/8;
+    int tap_num = tap%8 + 1;
+
+    switch (tap_dir) {
+        case 1:
+            printf("INV_TAP_AXIS_X_POS\n");
+            break;
+        case 2:
+            printf("INV_TAP_AXIS_X_NEG\n");
+            break;
+        case 3:
+            printf("INV_TAP_AXIS_Y_POS\n");
+            break;
+        case 4:
+            printf("INV_TAP_AXIS_Y_NEG\n");
+            break;
+        case 5:
+            printf("INV_TAP_AXIS_Z_POS\n");
+            break;
+        case 6:
+            printf("INV_TAP_AXIS_Z_NEG\n");
+            break;
+        default:
+            break;
+    }
+    printf("Tap number: %d\n", tap_num);
+}
+
+static void read_compass(int event_number) 
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned int RX;
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", event_number);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open compass\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1) {
+    clock_gettime(CLOCK_MONOTONIC, &tsp);
+        /*read compass data here */
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("CX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("CY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("CZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("GES?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+
+static void read_gesture(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    MPL_LOGI("%s\n", file_name);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gusture.\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while(1){
+        clock_gettime(CLOCK_MONOTONIC, &tsp);
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL;
+        //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3);
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL;
+                printf("mono=%lld\n", time);
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_RX:
+                printf("GESX:%5x\n", ev[ii].value);
+                HandleTap(ev[ii].value);
+                break;
+            case REL_RY:
+                printf("GESY:%5x\n", ev[ii].value);
+                HandleOrient(ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("FLICK:%5x\n", ev[ii].value);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+        }
+}
+
+static void read_gyro_accel(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    unsigned int RX;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gyro/accel\n");
+        return;
+    }    
+        
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1){
+        //usleep(20000);
+        ret_byte = read(fd, ev, ev_size);
+        if (ret_byte < 0)
+            continue;
+        //ret_byte = 0;
+
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("T: %lld diff=%d ", time0, (int)(time1 - time0));
+                clock_gettime(CLOCK_MONOTONIC, &tsp);
+                time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+                printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("GX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("GY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("GZ:%5d ", ev[ii].value);
+                break;
+            case REL_RX:
+                printf("AX:%5d ", ev[ii].value);
+                break;
+            case REL_RY:
+                printf("AY:%5d ", ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("AZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "r");
+    if (!fp)
+        return -errno;
+    count = fread(data, 1, num_bytes, fp);
+    fclose(fp);
+    return count;
+}
+
+void enable_flick(char *p)
+{
+    char sysfs_file[200];
+    printf("flick:%s\n", p);
+    sprintf(sysfs_file, "%s/flick_int_on", p);
+    inv_sysfs_write(sysfs_file, 1);
+    sprintf(sysfs_file, "%s/flick_upper", p);
+    inv_sysfs_write(sysfs_file, 3147790);
+    sprintf(sysfs_file, "%s/flick_lower", p);
+    inv_sysfs_write(sysfs_file, -3147790);
+    sprintf(sysfs_file, "%s/flick_counter", p);
+    inv_sysfs_write(sysfs_file, 50);
+    sprintf(sysfs_file, "%s/flick_message_on", p);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/flick_axis", p);
+    inv_sysfs_write(sysfs_file, 2);
+}
+
+void setup_dmp(char *sysfs_path)
+{
+    char sysfs_file[200];
+    char firmware_loaded[200], dmp_path[200];
+    char dd[10];
+    
+    inv_get_dmpfile(dmp_path);
+    sprintf(sysfs_file, "%s/fifo_rate", sysfs_path);
+    inv_sysfs_write(sysfs_file, 200);    
+    sprintf(sysfs_file, "%s/FSR", sysfs_path);
+    inv_sysfs_write(sysfs_file, 2000);    
+    sprintf(sysfs_file, "%s/accl_fs", sysfs_path);
+    inv_sysfs_write(sysfs_file, 4);
+    /*
+    sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded");
+    printf("%s\n", firmware_loaded);
+    inv_sysfs_write(firmware_loaded, 0);
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("beforefirmware_loaded=%c\n", dd[0]);
+
+    if ((fd = open(dmp_path, O_WRONLY)) < 0 ) {
+        perror("dmp fail");
+    }
+    inv_load_dmp(fd);
+    close(fd);
+    */
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("firmware_loaded=%c\n", dd[0]);
+}
+void read_pedometer(char *sysfs_path){
+    int steps;
+    char sysfs_file[200];
+    char dd[4];
+    sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path);
+    inv_sysfs_read(sysfs_file, 4, dd);
+    steps = dd[0] << 8 | dd[1];
+    printf("fff=%d\n", steps);
+}
+/* The running sequence:
+    "input_gyro 2 &". 
+    This will setup the dmp firmware and let it run on background. 
+    tap and flick will work at this time. 
+    To see accelerometer data and gyro data.
+    type :
+    "input_gyro ". 
+    This will print out gyro data and accelerometer data
+    To see Compass data
+    type:
+    "input_gyro 1" */
+
+int main(int argc, char *argv[])
+{
+    unsigned int RX, i, sel;
+    unsigned char key[16];
+    struct timeval tv;
+    struct timespec tsp0, tsp1, tsp2, tsp3;
+    int event_num;
+    char sysfs_path[200];
+    char chip_name[20];
+    char sysfs_file[200];
+    if (INV_SUCCESS != inv_check_key()) {
+        printf("key check fail\n");
+        exit(0);
+    }else
+        printf("key authenticated\n");
+
+    for(i=0;i<16;i++){
+        key[i] = 0xff;
+    }
+    RX = inv_get_sysfs_key(key);
+    if(RX == INV_SUCCESS){
+        for(i=0;i<16;i++){
+            printf("%d, ", key[i]);
+        }
+        printf("\n");
+    }else{
+        printf("get key failed\n");
+    }
+    memset(sysfs_path, 0, 200);
+    memset(sysfs_file, 0, 200);
+    memset(chip_name, 0, 20);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_chip_name(chip_name);
+    printf("sysfs path: %s\n", sysfs_path);
+    printf("chip name: %s\n", chip_name);
+    /*set up driver*/
+    sprintf(sysfs_file, "%s/enable", sysfs_path);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/power_state", sysfs_path);
+    inv_sysfs_write(sysfs_file, 1);    
+    if ((getuid ()) != 0)
+        printf ("You are not root! This may not work...\n");
+    
+    if(argc ==2 )
+        sel = argv[1][0] - 0x30;
+    else
+        sel = 0;
+    switch(sel){
+    case 0:
+        printf("-------------------------------\n");
+        printf("--- log gyro and accel data ---\n");
+        printf("-------------------------------\n");
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number(chip_name, &event_num) < 0)
+            printf("mpu not installed\n");
+        else
+            read_gyro_accel(event_num);
+         break;
+         
+    case 1:
+        printf("------------------------\n");
+        printf("--- log compass data ---\n");
+        printf("------------------------\n");
+        sprintf(sysfs_file, "%s/compass_enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_COMPASS", &event_num) < 0)
+            printf("compass is not enabled\n");
+        else
+            read_compass(event_num);
+        break;
+        
+    case 2:
+        printf("--------------------\n");
+        printf("--- log gestures ---\n");
+        printf("--------------------\n");
+        setup_dmp(sysfs_path);
+        enable_flick(sysfs_path);
+        sprintf(sysfs_file, "%s/tap_on", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_DMP", &event_num) < 0)
+            printf("DMP not enabled\n");
+        else
+            read_gesture(event_num);
+        break;
+        
+    case 3:
+        printf("-----------------\n");
+        printf("--- pedometer ---\n");
+        printf("-----------------\n");
+        read_pedometer(sysfs_path);
+        break;
+        
+    default:
+        printf("error choice\n");
+        break;
+    }
+    
+    gettimeofday(&tv, NULL);
+    clock_gettime(CLOCK_MONOTONIC, &tsp1);
+    clock_gettime(CLOCK_REALTIME, &tsp0);
+
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2);
+    clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3);
+    //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC,  CLOCK_REALTIME,
+    //    CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID);
+    //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec);
+    //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec);
+
+    //ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    //printf ("Reading From : %s (%s)\n", argv[1], name);
+
+
+    return 0;
+}
+
diff --git a/libsensors/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors/software/simple_apps/self_test/build/android/inv_self_test-shared
new file mode 100644
index 0000000..33c9eef
--- /dev/null
+++ b/libsensors/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/libsensors/software/simple_apps/self_test/build/android/shared.mk b/libsensors/software/simple_apps/self_test/build/android/shared.mk
new file mode 100644
index 0000000..3a055cc
--- /dev/null
+++ b/libsensors/software/simple_apps/self_test/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = inv_self_test$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors/software/simple_apps/self_test/build/filelist.mk b/libsensors/software/simple_apps/self_test/build/filelist.mk
new file mode 100644
index 0000000..492f47e
--- /dev/null
+++ b/libsensors/software/simple_apps/self_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for console_test ####
+
+# headers
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/inv_self_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors/software/simple_apps/self_test/inv_self_test.c b/libsensors/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..4f9996c
--- /dev/null
+++ b/libsensors/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,264 @@
+/**
+ *  Self Test application for Invensense's MPU6050/MPU9150.
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+
+#ifndef ABS
+#define ABS(x)(((x) >= 0) ? (x) : -(x))
+#endif
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53394
+
+#define FALSE   0
+#define TRUE    1
+
+#define GYRO_PASS_STATUS_BIT    0x01
+#define ACCEL_PASS_STATUS_BIT   0x02
+#define COMPASS_PASS_STATUS_BIT 0x04
+
+typedef union {
+    long l; 
+    int i;
+} bias_dtype;
+
+struct inv_sysfs_names_s {
+    char *enable;
+    char *power_state;
+    char *self_test;
+    char *temperature;
+    char *accl_bias;
+};
+
+const struct inv_sysfs_names_s mpu= {
+     /* MPU6050 & MPU9150 */
+    .enable         = "/sys/class/invensense/mpu/enable",
+    .power_state    = "/sys/class/invensense/mpu/power_state",
+    .self_test      = "/sys/class/invensense/mpu/self_test",
+    .temperature    = "/sys/class/invensense/mpu/temperature",
+    .accl_bias      = "/sys/class/invensense/mpu/accl_bias"
+};
+
+struct inv_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+};
+
+static struct inv_db_save_t save_data;
+
+/** This function receives the data that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&save_data, data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &save_data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+
+/**
+ *  Main Self test 
+ */
+int main(int argc, char **argv)
+{
+    FILE *fptr;
+    int self_test_status;
+    inv_error_t result;
+    bias_dtype gyro_bias[3];
+    bias_dtype accel_bias[3];
+    int axis = 0;
+    size_t packet_sz;
+    int axis_sign = 1;
+    unsigned char *buffer;
+    long timestamp;
+    int temperature=0;
+
+    // Initialize storage manager
+    inv_init_storage_manager();
+
+    // Clear out data.
+    memset(&save_data, 0, sizeof(save_data));
+    memset(gyro_bias,0, sizeof(gyro_bias));
+    memset(accel_bias,0, sizeof(accel_bias));
+
+    // Register packet to be saved.
+    result = inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                     sizeof(save_data),
+                                     INV_DB_SAVE_KEY);
+
+    // Power ON MPUxxxx chip
+    if (inv_sysfs_write(mpu.power_state, 1) <0) {
+        printf("ERR- Failed to set power state=1\n");
+    } else {
+        // Note: Driver turns on power automatically when self-test invoked
+    }
+
+    fptr = fopen(mpu.self_test, "r");
+    if (fptr != NULL) {
+        // Invoke self-test and read gyro bias
+        fscanf(fptr, "%d,%d,%d,%d",
+               &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status);
+
+        printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
+                      (self_test_status & GYRO_PASS_STATUS_BIT), 
+                      (self_test_status & ACCEL_PASS_STATUS_BIT) >>1,
+                      (self_test_status & COMPASS_PASS_STATUS_BIT) >>2);
+        printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
+        fclose(fptr);
+        
+        if (!(self_test_status & GYRO_PASS_STATUS_BIT)) {
+            // Reset gyro bias data if gyro self-test failed
+            memset(gyro_bias,0, sizeof(gyro_bias));
+            printf("Self-Test:Failed Gyro self-test\n");
+        }
+
+        if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+            // Read Accel Bias
+            fptr= fopen(mpu.accl_bias, "r");
+            if (fptr != NULL) {
+                fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i);
+                printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+                fclose(fptr);
+            } else {
+                printf("Self-Test:ERR-Couldn't read accel bias\n");
+            }
+        } else {
+            memset(accel_bias,0, sizeof(accel_bias));
+            printf("Self-Test:Failed Accel self-test\n");
+        }
+
+        // Read temperature
+        if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT))
+        {
+            fptr= fopen(mpu.temperature, "r");
+            if (fptr != NULL) {
+                fscanf(fptr,"%d %ld", &temperature, &timestamp);
+                fclose(fptr);
+            } else {
+                printf("Self-Test:ERR-Couldn't read temperature\n");
+            }
+        }
+        
+    } else {
+        printf("Self-Test:ERR-Couldn't invoke self-test\n");
+    }
+
+    // When we read gyro bias, the bias is in raw units scaled by 1000.
+    // We store the bias in raw units scaled by 2^16
+    save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f);
+    save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f);
+    save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f);
+
+    // Save temperature @ time stored.  Temperature is in degrees Celsius scaled by 2^16
+    save_data.gyro_temp = temperature * (1L << 16);
+    save_data.accel_temp = save_data.gyro_temp; 
+
+    // When we read accel bias, the bias is in raw units scaled by 1000.
+    // and it contains the gravity vector.
+    
+    // Find the orientation of the device, by looking for gravity
+    if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) {
+        axis = 1;
+    }
+    if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) {
+        axis = 2;
+    }
+    if (accel_bias[axis].l < 0) {
+        axis_sign = -1;
+    }
+    
+    // Remove gravity, gravity in raw units should be 16384 for a 2g setting.
+    // We read data scaled by 1000, so 
+    accel_bias[axis].l -= axis_sign * 4096L * 1000L;
+
+    // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
+    save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
+
+#if 1
+    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0],
+                    save_data.accel_bias[1], save_data.accel_bias[2]);
+    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0],
+                    save_data.gyro_bias[1], save_data.gyro_bias[2]);
+    printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp);
+    printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp);
+#endif
+
+    // Get size of packet to store.
+    inv_get_mpl_state_size(&packet_sz);
+
+    // Create place to store data
+    buffer = (unsigned char *)malloc(packet_sz + 10);
+    if (buffer == NULL) {
+        printf("Self-Test:Can't allocate buffer\n");
+        return -1;
+    }
+
+    result = inv_save_mpl_states(buffer, packet_sz);
+    if (result) {
+        result= -1;
+    } else {
+        fptr= fopen(MLCAL_FILE, "wb+");
+        if (fptr != NULL) {
+            fwrite(buffer, 1, packet_sz, fptr);
+            fclose(fptr);
+        } else {
+            printf("Self-Test:ERR- Can't open calibration file to write - %s\n", 
+                   MLCAL_FILE);
+            result= -1;
+        }
+    }
+
+    free(buffer);
+    return result;
+}
+
diff --git a/mlsdk/Android.mk b/mlsdk/Android.mk
deleted file mode 100644
index 41b1457..0000000
--- a/mlsdk/Android.mk
+++ /dev/null
@@ -1,104 +0,0 @@
-LOCAL_PATH := $(call my-dir)
-
-ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_MODULE := libmlplatform
-#modify these to point to the mpl source installation
-MLSDK_ROOT = .
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform/linux
-
-LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/kernel
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite
-
-ML_SOURCES := \
-    $(MLPLATFORM_DIR)/mlos_linux.c \
-    $(MLPLATFORM_DIR)/mlsl_linux_mpu.c
-
-LOCAL_SRC_FILES := $(ML_SOURCES)
-
-LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils
-LOCAL_PRELINK_MODULE := false
-include $(BUILD_SHARED_LIBRARY)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE := libmllite
-LOCAL_MODULE_TAGS := optional
-#modify these to point to the mpl source installation
-MLSDK_ROOT = .
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
-MLLITE_DIR = $(MLSDK_ROOT)/mllite
-MPL_DIR = $(MLSDK_ROOT)/mldmp
-
-LOCAL_CFLAGS += -DNDEBUG
-LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
-LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE
-LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\"
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MPL_DIR) -I$(LOCAL_PATH)/$(MLLITE_DIR) -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/include
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlutils -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlapps/common
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite/akmd
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/linux
-
-# optionally apply the compass filter. this is set in
-# BoardConfig.mk
-ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true)
-LOCAL_CFLAGS += -DAPPLY_COMPASS_FILTER
-endif
-
-ML_SOURCES = \
-        $(MLLITE_DIR)/accel.c \
-        $(MLLITE_DIR)/compass.c \
-        $(MLLITE_DIR)/pressure.c \
-        $(MLLITE_DIR)/mldl_cfg_mpu.c \
-        $(MLLITE_DIR)/dmpDefault.c \
-        $(MLLITE_DIR)/ml.c \
-	$(MLLITE_DIR)/mlarray.c \
-	$(MLLITE_DIR)/mlarray_legacy.c \
-        $(MLLITE_DIR)/mlFIFO.c \
-        $(MLLITE_DIR)/mlFIFOHW.c \
-        $(MLLITE_DIR)/mlMathFunc.c \
-        $(MLLITE_DIR)/ml_stored_data.c \
-        $(MLLITE_DIR)/mlcontrol.c \
-        $(MLLITE_DIR)/mldl.c \
-        $(MLLITE_DIR)/mldmp.c \
-        $(MLLITE_DIR)/mlstates.c \
-        $(MLLITE_DIR)/mlsupervisor.c \
-        $(MLLITE_DIR)/mlBiasNoMotion.c \
-        $(MLLITE_DIR)/mlSetGyroBias.c \
-        \
-        $(MLLITE_DIR)/ml_mputest.c \
-        $(MLSDK_ROOT)/mlutils/mputest.c \
-        $(MLSDK_ROOT)/mlutils/checksum.c
-
-
-ifeq ($(HARDWARE),M_HW)
-    ML_SOURCES += $(MLLITE_DIR)/accel/mantis.c
-endif
-
-LOCAL_SRC_FILES := $(ML_SOURCES)
-LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform
-LOCAL_PRELINK_MODULE := false
-include $(BUILD_SHARED_LIBRARY)
-
-#This makes an .so from our .a
-#include $(CLEAR_VARS)
-#LOCAL_MODULE := libmpl
-#LOCAL_MODULE_TAGS := optional
-#LOCAL_SRC_FILES := mlsdk/mldmp/mpl/android/libmpl.a
-#LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform libmllite
-#LOCAL_WHOLE_STATIC_LIBRARIES := libmpl
-#LOCAL_PREBUILT_LIBS := mlsdk/mldmp/mpl/android/libmpl.a
-#LOCAL_PRELINK_MODULE := false
-#include $(BUILD_SHARED_LIBRARY)
-#include $(BUILD_MULTI_PREBUILT)
-
-endif
diff --git a/mlsdk/mllite/accel.c b/mlsdk/mllite/accel.c
deleted file mode 100644
index 60b8d6c..0000000
--- a/mlsdk/mllite/accel.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: accel.c 4595 2011-01-25 01:43:03Z mcaramello $
- *
- *******************************************************************************/
-
-/** 
- *  @defgroup ACCELDL 
- *  @brief  Motion Library - Accel Driver Layer.
- *          Provides the interface to setup and handle an accel
- *          connected to either the primary or the seconday I2C interface 
- *          of the gyroscope.
- *
- *  @{
- *      @file   accel.c
- *      @brief  Accel setup and handling methods.
-**/
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "ml.h"
-#include "mlinclude.h"
-#include "dmpKey.h"
-#include "mlFIFO.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mlMathFunc.h"
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-accel"
-
-#define ACCEL_DEBUG 0
-
-/* --------------------- */
-/* - Global Variables. - */
-/* --------------------- */
-
-/* --------------------- */
-/* - Static Variables. - */
-/* --------------------- */
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* -------------- */
-/* - Externs.   - */
-/* -------------- */
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/** 
- *  @brief  Used to determine if an accel is configured and
- *          used by the MPL.
- *  @return INV_SUCCESS if the accel is present.
- */
-unsigned char inv_accel_present(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->accel &&
-        NULL != mldl_cfg->accel->resume &&
-        mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL)
-        return TRUE;
-    else
-        return FALSE;
-}
-
-/**
- *  @brief   Query the accel slave address.
- *  @return  The 7-bit accel slave address.
- */
-unsigned char inv_get_slave_addr(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->pdata)
-        return mldl_cfg->pdata->accel.address;
-    else
-        return 0;
-}
-
-/**
- *  @brief   Get the ID of the accel in use.
- *  @return  ID of the accel in use.
- */
-unsigned short inv_get_accel_id(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->accel) {
-        return mldl_cfg->accel->id;
-    }
-    return ID_INVALID;
-}
-
-/**
- *  @brief  Get a sample of accel data from the device.
- *  @param  data
- *              the buffer to store the accel raw data for
- *              X, Y, and Z axes.
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_accel_data(long *data)
-{
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    inv_error_t result;
-    unsigned char raw_data[2 * ACCEL_NUM_AXES];
-    long tmp[ACCEL_NUM_AXES];
-    int ii;
-    signed char *mtx = mldl_cfg->pdata->accel.orientation;
-    char accelId = mldl_cfg->accel->id;
-
-    if (NULL == data)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (mldl_cfg->accel->read_len > sizeof(raw_data))
-        return INV_ERROR_ASSERTION_FAILURE;
-
-    result = (inv_error_t) inv_mpu_read_accel(mldl_cfg,
-                                              inv_get_serial_handle(),
-                                              inv_get_serial_handle(),
-                                              raw_data);
-    if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
-        return result;
-    }
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
-        if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) {
-            tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256;
-            tmp[ii] += (long)((unsigned char)raw_data[2 * ii]);
-        } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) ||
-                   (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) {
-            tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256;
-            tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]);
-            if (accelId == ACCEL_ID_KXSD9) {
-                tmp[ii] = (long)((short)(((unsigned short)tmp[ii])
-                                         + ((unsigned short)0x8000)));
-            }
-        } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) {
-            tmp[ii] = (long)((signed char)raw_data[ii]) * 256;
-        } else {
-            result = INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-        }
-    }
-
-    for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
-        data[ii] = ((long)tmp[0] * mtx[3 * ii] +
-                    (long)tmp[1] * mtx[3 * ii + 1] +
-                    (long)tmp[2] * mtx[3 * ii + 2]);
-    }
-
-    //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]);
-    return result;
-}
-
-/**
- *  @}
- */
diff --git a/mlsdk/mllite/accel.h b/mlsdk/mllite/accel.h
deleted file mode 100644
index d3fbc6a..0000000
--- a/mlsdk/mllite/accel.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: accel.h 4580 2011-01-22 03:19:23Z prao $
- *
- *******************************************************************************/
-
-#ifndef ACCEL_H
-#define ACCEL_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "accel_legacy.h"
-#endif
-
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-
-    unsigned char inv_accel_present(void);
-    unsigned char inv_get_slave_addr(void);
-    inv_error_t inv_get_accel_data(long *data);
-    unsigned short inv_get_accel_id(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // ACCEL_H
diff --git a/mlsdk/mllite/compass.c b/mlsdk/mllite/compass.c
deleted file mode 100644
index 798cb9f..0000000
--- a/mlsdk/mllite/compass.c
+++ /dev/null
@@ -1,579 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *******************************************************************************/
-
-/** 
- *  @defgroup COMPASSDL 
- *  @brief  Motion Library - Compass Driver Layer.
- *          Provides the interface to setup and handle an compass
- *          connected to either the primary or the seconday I2C interface 
- *          of the gyroscope.
- *
- *  @{
- *      @file   compass.c
- *      @brief  Compass setup and handling methods.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-#include <stdlib.h>
-#include "compass.h"
-
-#include "ml.h"
-#include "mlinclude.h"
-#include "dmpKey.h"
-#include "mlFIFO.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mlMathFunc.h"
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-compass"
-
-#define COMPASS_DEBUG 0
-
-/* --------------------- */
-/* - Global Variables. - */
-/* --------------------- */
-
-/* --------------------- */
-/* - Static Variables. - */
-/* --------------------- */
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* -------------- */
-/* - Externs.   - */
-/* -------------- */
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-static float square(float data)
-{
-    return data * data;
-}
-
-static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
-{
-    int i;
-
-    adap_filter->num = 0;
-    adap_filter->index = 0;
-    adap_filter->noise = noise;
-    adap_filter->len = len;
-
-    for (i = 0; i < adap_filter->len; ++i) {
-        adap_filter->sequence[i] = 0;
-    }
-}
-
-static int cmpfloat(const void *p1, const void *p2)
-{
-    return *(float*)p1 - *(float*)p2;
-}
-
-
-static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
-{
-    float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
-    int i;
-
-    if (adap_filter->len <= 1) {
-        return in;
-    }
-    if (adap_filter->num < adap_filter->len) {
-        adap_filter->sequence[adap_filter->index++] = in;
-        adap_filter->num++;
-        return in;
-    }
-    if (adap_filter->len <= adap_filter->index) {
-        adap_filter->index = 0;
-    }
-    adap_filter->sequence[adap_filter->index++] = in;
-
-    avg = 0;
-    for (i = 0; i < adap_filter->len; i++) {
-        avg += adap_filter->sequence[i];
-    }
-    avg /= adap_filter->len;
-
-    memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
-    qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
-    median = sorted[adap_filter->len/2];
-
-    sum = 0;
-    for (i = 0; i < adap_filter->len; i++) {
-        sum += square(avg - adap_filter->sequence[i]);
-    }
-    sum /= adap_filter->len;
-
-    if (sum <= adap_filter->noise) {
-        return median;
-    }
-
-    return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
-}
-
-static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
-{
-    thresh_filter->threshold = threshold;
-    thresh_filter->last = 0;
-}
-
-static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
-{
-    if (in < thresh_filter->last - thresh_filter->threshold
-            || thresh_filter->last + thresh_filter->threshold < in) {
-        thresh_filter->last = in;
-        return in;
-    }
-    else {
-        return thresh_filter->last;
-    }
-}
-
-static int init(yas_filter_handle_t *t)
-{
-    float noise[] = {
-        YAS_DEFAULT_FILTER_NOISE,
-        YAS_DEFAULT_FILTER_NOISE,
-        YAS_DEFAULT_FILTER_NOISE,
-    };
-    int i;
-
-    if (t == NULL) {
-        return -1;
-    }
-
-    for (i = 0; i < 3; i++) {
-        adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
-        thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
-    }
-
-    return 0;
-}
-
-static int update(yas_filter_handle_t *t, float *input, float *output)
-{
-    int i;
-
-    if (t == NULL || input == NULL || output == NULL) {
-        return -1;
-    }
-
-    for (i = 0; i < 3; i++) {
-        output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
-        output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
-    }
-
-    return 0;
-}
-
-int yas_filter_init(yas_filter_if_s *f)
-{
-    if (f == NULL) {
-        return -1;
-    }
-    f->init = init;
-    f->update = update;
-
-    return 0;
-}
-
-/**
- *  @brief  Used to determine if a compass is
- *          configured and used by the MPL.
- *  @return INV_SUCCESS if the compass is present.
- */
-unsigned char inv_compass_present(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->compass &&
-        NULL != mldl_cfg->compass->resume &&
-        mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
-        return TRUE;
-    else
-        return FALSE;
-}
-
-/**
- *  @brief   Query the compass slave address.
- *  @return  The 7-bit compass slave address.
- */
-unsigned char inv_get_compass_slave_addr(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->pdata)
-        return mldl_cfg->pdata->compass.address;
-    else
-        return 0;
-}
-
-/**
- *  @brief   Get the ID of the compass in use.
- *  @return  ID of the compass in use.
- */
-unsigned short inv_get_compass_id(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->compass) {
-        return mldl_cfg->compass->id;
-    }
-    return ID_INVALID;
-}
-
-/**
- *  @brief  Get a sample of compass data from the device.
- *  @param  data
- *              the buffer to store the compass raw data for
- *              X, Y, and Z axes.
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_compass_data(long *data)
-{
-    inv_error_t result;
-    int ii;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    unsigned char *tmp = inv_obj.compass_raw_data;
-
-    if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
-        return INV_ERROR_INVALID_CONFIGURATION;
-    }
-
-    if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
-        !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
-        /*--- read the compass sensor data.
-          The compass read function may return an INV_ERROR_COMPASS_* errors
-          when the data is not ready (read/refresh frequency mismatch) or 
-          the internal data sampling timing of the device was not respected. 
-          Returning the error code will make the sensor fusion supervisor 
-          ignore this compass data sample. ---*/
-        result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
-                                                    inv_get_serial_handle(),
-                                                    inv_get_serial_handle(),
-                                                    tmp);
-        if (result) {
-            if (COMPASS_DEBUG) {
-                MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
-            }
-            return result;
-        }
-        for (ii = 0; ii < 3; ii++) {
-            if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
-                data[ii] =
-                    ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
-            else
-                data[ii] =
-                    ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
-        }
-
-        inv_obj.compass_overunder = (int)tmp[6];
-
-    } else {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 ||             \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-        result = inv_get_external_sensor_data(data, 3);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-        {
-            static unsigned char first = TRUE;
-            // one-off write to AKM
-            if (first) {
-                unsigned char regs[] = {
-                    // beginning Mantis register for one-off slave R/W
-                    MPUREG_I2C_SLV4_ADDR,
-                    // the slave to write to
-                    mldl_cfg->pdata->compass.address,
-                    // the register to write to
-                    /*mldl_cfg->compass->trigger->reg */ 0x0A,
-                    // the value to write
-                    /*mldl_cfg->compass->trigger->value */ 0x01,
-                    // enable the write
-                    0xC0
-                };
-                result =
-                    inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
-                                     ARRAY_SIZE(regs), regs);
-                first = FALSE;
-            } else {
-                unsigned char regs[] = {
-                    MPUREG_I2C_SLV4_CTRL,
-                    0xC0
-                };
-                result =
-                    inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
-                                     ARRAY_SIZE(regs), regs);
-            }
-        }
-#endif
-#else
-        return INV_ERROR_INVALID_CONFIGURATION;
-#endif                          // CONFIG_MPU_SENSORS_xxxx
-    }
-    data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
-    data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
-    data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
-
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief  Sets the compass bias.
- *  @param  bias 
- *              Compass bias, length 3. Scale is micro Tesla's * 2^16. 
- *              Frame is mount frame which may be different from body frame.
- *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
- */
-inv_error_t inv_set_compass_bias(long *bias)
-{
-    inv_error_t result = INV_SUCCESS;
-    long biasC[3];
-    struct mldl_cfg *mldlCfg = inv_get_dl_config();
-
-    inv_obj.compass_bias[0] = bias[0];
-    inv_obj.compass_bias[1] = bias[1];
-    inv_obj.compass_bias[2] = bias[2];
-
-    // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
-    biasC[0] =
-        (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
-        inv_obj.init_compass_bias[0] * (1L << 16);
-    biasC[1] =
-        (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
-        inv_obj.init_compass_bias[1] * (1L << 16);
-    biasC[2] =
-        (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
-        inv_obj.init_compass_bias[2] * (1L << 16);
-
-    if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
-        unsigned char reg[4];
-        long biasB[3];
-        signed char *orC = mldlCfg->pdata->compass.orientation;
-
-        // Now transform to body frame
-        biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
-        biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
-        biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
-
-        result =
-            inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
-                               inv_int32_to_big8(biasB[0], reg));
-        result =
-            inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
-                               inv_int32_to_big8(biasB[1], reg));
-        result =
-            inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
-                               inv_int32_to_big8(biasB[2], reg));
-    }
-    return result;
-}
-
-/**
- *  @brief  Write a single register on the compass slave device, regardless
- *          of the bus it is connected to and the MPU's configuration.
- *  @param  reg
- *              the register to write to on the slave compass device.
- *  @param  val
- *              the value to write.
- *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
- */
-inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
-{
-    struct ext_slave_config config;
-    unsigned char data[2];
-    inv_error_t result;
-
-    data[0] = reg;
-    data[1] = val;
-
-    config.key = MPU_SLAVE_WRITE_REGISTERS;
-    config.len = 2;
-    config.apply = TRUE;
-    config.data = data;
-
-    result = inv_mpu_config_compass(inv_get_dl_config(),
-                                    inv_get_serial_handle(),
-                                    inv_get_serial_handle(), &config);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- *  @brief  Read values from the compass slave device registers, regardless
- *          of the bus it is connected to and the MPU's configuration.
- *  @param  reg
- *              the register to read from on the slave compass device.
- *  @param  val
- *              a buffer of 3 elements to store the values read from the 
- *              compass device.
- *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
- */
-inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
-{
-    struct ext_slave_config config;
-    unsigned char data[2];
-    inv_error_t result;
-
-    data[0] = reg;
-
-    config.key = MPU_SLAVE_READ_REGISTERS;
-    config.len = 2;
-    config.apply = TRUE;
-    config.data = data;
-
-    result = inv_mpu_get_compass_config(inv_get_dl_config(),
-                                        inv_get_serial_handle(),
-                                        inv_get_serial_handle(), &config);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    *val = data[1];
-    return result;
-}
-
-/**
- *  @brief  Read values from the compass slave device scale registers, regardless
- *          of the bus it is connected to and the MPU's configuration.
- *  @param  reg
- *              the register to read from on the slave compass device.
- *  @param  val
- *              a buffer of 3 elements to store the values read from the 
- *              compass device.
- *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
- */
-inv_error_t inv_compass_read_scale(long *val)
-{
-    struct ext_slave_config config;
-    unsigned char data[3];
-    inv_error_t result;
-
-    config.key = MPU_SLAVE_READ_SCALE;
-    config.len = 3;
-    config.apply = TRUE;
-    config.data = data;
-
-    result = inv_mpu_get_compass_config(inv_get_dl_config(),
-                                        inv_get_serial_handle(),
-                                        inv_get_serial_handle(), &config);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    val[0] = ((data[0] - 128) << 22) + (1L << 30);
-    val[1] = ((data[1] - 128) << 22) + (1L << 30);
-    val[2] = ((data[2] - 128) << 22) + (1L << 30);
-    return result;
-}
-
-inv_error_t inv_set_compass_offset(void)
-{
-    struct ext_slave_config config;
-    unsigned char data[3];
-    inv_error_t result;
-
-    config.key = MPU_SLAVE_OFFSET_VALS;
-    config.len = 3;
-    config.apply = TRUE;
-    config.data = data;
-
-    if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
-        /* push stored values */
-        data[0] = (char)inv_obj.compass_offsets[0];
-        data[1] = (char)inv_obj.compass_offsets[1];
-        data[2] = (char)inv_obj.compass_offsets[2];
-        MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
-        result = inv_mpu_config_compass(inv_get_dl_config(),
-                                        inv_get_serial_handle(),
-                                        inv_get_serial_handle(), &config);
-    } else {
-        /* compute new values and store them */
-        result = inv_mpu_get_compass_config(inv_get_dl_config(),
-                                            inv_get_serial_handle(),
-                                            inv_get_serial_handle(), &config);
-        MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
-        if(result == INV_SUCCESS) {
-            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
-            inv_obj.compass_offsets[0] = data[0];
-            inv_obj.compass_offsets[1] = data[1];
-            inv_obj.compass_offsets[2] = data[2];
-        }
-    }
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-inv_error_t inv_compass_check_range(void)
-{
-    struct ext_slave_config config;
-    unsigned char data[3];
-    inv_error_t result;
-
-    config.key = MPU_SLAVE_RANGE_CHECK;
-    config.len = 3;
-    config.apply = TRUE;
-    config.data = data;
-
-    result = inv_mpu_get_compass_config(inv_get_dl_config(),
-                                        inv_get_serial_handle(),
-                                        inv_get_serial_handle(), &config);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    if(data[0] || data[1] || data[2]) {
-        /* some value clipped */
-        return INV_ERROR_COMPASS_DATA_ERROR;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/mlsdk/mllite/compass.h b/mlsdk/mllite/compass.h
deleted file mode 100644
index f0bdb58..0000000
--- a/mlsdk/mllite/compass.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: compass.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef COMPASS_H
-#define COMPASS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "compass_legacy.h"
-#endif
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-#define YAS_MAX_FILTER_LEN (20)
-#define YAS_DEFAULT_FILTER_LEN (20)
-#define YAS_DEFAULT_FILTER_THRESH (300) /* 300 nT */
-#define YAS_DEFAULT_FILTER_NOISE (2000 * 2000) /* standard deviation 2000 nT */
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-struct yas_adaptive_filter {
-    int num;
-    int index;
-    int len;
-    float noise;
-    float sequence[YAS_MAX_FILTER_LEN];
-};
-
-struct yas_thresh_filter {
-    float threshold;
-    float last;
-};
-
-typedef struct {
-    struct yas_adaptive_filter adap_filter[3];
-    struct yas_thresh_filter thresh_filter[3];
-} yas_filter_handle_t;
-
-typedef struct {
-    int (*init)(yas_filter_handle_t *t);
-    int (*update)(yas_filter_handle_t *t, float *input, float *output);
-} yas_filter_if_s;
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-
-    unsigned char inv_compass_present(void);
-    unsigned char inv_get_compass_slave_addr(void);
-    inv_error_t inv_get_compass_data(long *data);
-    inv_error_t inv_set_compass_bias(long *bias);
-    unsigned short inv_get_compass_id(void);
-    inv_error_t inv_set_compass_offset(void);
-    inv_error_t inv_compass_check_range(void);
-    inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val);
-    inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val);
-    inv_error_t inv_compass_read_scale(long *val);
-
-    int yas_filter_init(yas_filter_if_s *f);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // COMPASS_H
diff --git a/mlsdk/mllite/dmpDefault.c b/mlsdk/mllite/dmpDefault.c
deleted file mode 100644
index fe29376..0000000
--- a/mlsdk/mllite/dmpDefault.c
+++ /dev/null
@@ -1,416 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/***************************************************************************** *
- * $Id: dmpDefault.c 5627 2011-06-10 22:34:18Z nroyer $ 
- ******************************************************************************/
-
-/* WARNING: autogenerated code, do not modify */
-/**
- *  @defgroup DMPDEFAULT 
- *  @brief    Data and configuration for MLDmpDefaultOpen.
- *
- *  @{
- *      @file   inv_setup_dmp.c
- *      @brief  Data and configuration for MLDmpDefaultOpen.
- */
-
-#include "mltypes.h"
-#include "dmpDefault.h"
-#include "dmpKey.h"
-#include "dmpmap.h"
-#include "ml.h"
-#include "mpu.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-
-#define CFG_25  703
-#define CFG_24  699
-#define CFG_26  707
-#define CFG_21  802
-#define CFG_20  645
-#define CFG_23  814
-#define CFG_TAP4  808
-#define CFG_TAP5  809
-#define CFG_TAP6  810
-#define CFG_1  783
-#define CFG_TAP0  802
-#define CFG_TAP1  804
-#define CFG_TAP2  805
-#define CFG_TAP3  806
-#define FCFG_AZ  878
-#define CFG_ORIENT_IRQ_1  715
-#define CFG_ORIENT_IRQ_2  738
-#define CFG_ORIENT_IRQ_3  743
-#define CFG_TAP_QUANTIZE  647
-#define FCFG_3  936
-#define CFG_TAP_CLEAR_STICKY  817
-#define FCFG_1  868
-#define CFG_ACCEL_FILTER  968
-#define FCFG_2  872
-#define CFG_3D  521
-#define CFG_3B  517
-#define CFG_3C  519
-#define FCFG_5  942
-#define FCFG_4  857
-#define FCFG_FSCALE  877
-#define CFG_TAP_JERK  639
-#define FCFG_6  996
-#define CFG_12  797
-#define FCFG_7  930
-#define CFG_14  790
-#define CFG_15  790
-#define CFG_16  815
-#define CFG_18  551
-#define CFG_6  823
-#define CFG_7  564
-#define CFG_4  526
-#define CFG_5  749
-#define CFG_3  515
-#define CFG_GYRO_SOURCE  777
-#define CFG_8  772
-#define CFG_9  778
-#define CFG_ORIENT_2  733
-#define CFG_ORIENT_1  713
-#define FCFG_ACCEL_INPUT  904
-#define CFG_TAP7  811
-#define CFG_TAP_SAVE_ACCB  687
-#define FCFG_ACCEL_INIT  831
-
-
-#define D_0_22  (22)
-#define D_0_24  (24)
-#define D_0_36  (36)
-#define D_0_52  (52)
-#define D_0_96  (96)
-#define D_0_104 (104)
-#define D_0_108 (108)
-#define D_0_163 (163)
-#define D_0_188 (188)
-#define D_0_192 (192)
-#define D_0_224 (224)
-#define D_0_228 (228)
-#define D_0_232 (232)
-#define D_0_236 (236)
-
-#define D_1_2 (256 + 2)
-#define D_1_4 (256 + 4)
-#define D_1_8 (256 + 8)
-#define D_1_10 (256 + 10)
-#define D_1_24 (256 + 24)
-#define D_1_28 (256 + 28)
-#define D_1_92 (256 + 92)
-#define D_1_96 (256 + 96)
-#define D_1_98 (256 + 98)
-#define D_1_106 (256 + 106)
-#define D_1_108 (256 + 108)
-#define D_1_112 (256 + 112)
-#define D_1_128 (256 + 144)
-#define D_1_152 (256 + 12)
-#define D_1_168 (256 + 168)
-#define D_1_175 (256 + 175)
-#define D_1_178 (256 + 178)
-#define D_1_236 (256 + 236)
-#define D_1_244 (256 + 244)
-
-
-static const tKeyLabel dmpTConfig[] = {
-    {KEY_CFG_25, CFG_25},
-    {KEY_CFG_24, CFG_24},
-    {KEY_CFG_26, CFG_26},
-    {KEY_CFG_21, CFG_21},
-    {KEY_CFG_20, CFG_20},
-    {KEY_CFG_23, CFG_23},
-    {KEY_CFG_TAP4, CFG_TAP4},
-    {KEY_CFG_TAP5, CFG_TAP5},
-    {KEY_CFG_TAP6, CFG_TAP6},
-    {KEY_CFG_1, CFG_1},
-    {KEY_CFG_TAP0, CFG_TAP0},
-    {KEY_CFG_TAP1, CFG_TAP1},
-    {KEY_CFG_TAP2, CFG_TAP2},
-    {KEY_CFG_TAP3, CFG_TAP3},
-    {KEY_FCFG_AZ, FCFG_AZ},
-    {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1},
-    {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2},
-    {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3},
-    {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE},
-    {KEY_FCFG_3, FCFG_3},
-    {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY},
-    {KEY_FCFG_1, FCFG_1},
-    //{KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER},
-    {KEY_FCFG_2, FCFG_2},
-    {KEY_CFG_3D, CFG_3D},
-    {KEY_CFG_3B, CFG_3B},
-    {KEY_CFG_3C, CFG_3C},
-    {KEY_FCFG_5, FCFG_5},
-    {KEY_FCFG_4, FCFG_4},
-    {KEY_FCFG_FSCALE, FCFG_FSCALE},
-    {KEY_CFG_TAP_JERK, CFG_TAP_JERK},
-    {KEY_FCFG_6, FCFG_6},
-    {KEY_CFG_12, CFG_12},
-    {KEY_FCFG_7, FCFG_7},
-    {KEY_CFG_14, CFG_14},
-    {KEY_CFG_15, CFG_15},
-    {KEY_CFG_16, CFG_16},
-    {KEY_CFG_18, CFG_18},
-    {KEY_CFG_6, CFG_6},
-    {KEY_CFG_7, CFG_7},
-    {KEY_CFG_4, CFG_4},
-    {KEY_CFG_5, CFG_5},
-    {KEY_CFG_3, CFG_3},
-    {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE},
-    {KEY_CFG_8, CFG_8},
-    {KEY_CFG_9, CFG_9},
-    {KEY_CFG_ORIENT_2, CFG_ORIENT_2},
-    {KEY_CFG_ORIENT_1, CFG_ORIENT_1},
-    {KEY_FCFG_ACCEL_INPUT, FCFG_ACCEL_INPUT},
-    {KEY_CFG_TAP7, CFG_TAP7},
-    {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB},
-    {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT},
-
-    {KEY_D_0_22, D_0_22},
-    {KEY_D_0_24, D_0_24},
-    {KEY_D_0_36, D_0_36},
-    {KEY_D_0_52, D_0_52},
-    {KEY_D_0_96, D_0_96},
-    {KEY_D_0_104, D_0_104},
-    {KEY_D_0_108, D_0_108},
-    {KEY_D_0_163, D_0_163},
-    {KEY_D_0_188, D_0_188},
-    {KEY_D_0_192, D_0_192},
-    {KEY_D_0_224, D_0_224},
-    {KEY_D_0_228, D_0_228},
-    {KEY_D_0_232, D_0_232},
-    {KEY_D_0_236, D_0_236},
-
-    {KEY_DMP_PREVPTAT, DMP_PREVPTAT},
-    {KEY_D_1_2, D_1_2},
-    {KEY_D_1_4, D_1_4},
-    {KEY_D_1_8, D_1_8},
-    {KEY_D_1_10, D_1_10},
-    {KEY_D_1_24, D_1_24},
-    {KEY_D_1_28, D_1_28},
-    {KEY_D_1_92, D_1_92},
-    {KEY_D_1_96, D_1_96},
-    {KEY_D_1_98, D_1_98},
-    {KEY_D_1_106, D_1_106},
-    {KEY_D_1_108, D_1_108},
-    {KEY_D_1_112, D_1_112},
-    {KEY_D_1_128, D_1_128},
-    {KEY_D_1_152, D_1_152},
-    {KEY_D_1_168, D_1_168},
-    {KEY_D_1_175, D_1_175},
-    {KEY_D_1_178, D_1_178},
-    {KEY_D_1_236, D_1_236},
-    {KEY_D_1_244, D_1_244},
-
-    {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN},
-    {KEY_DMP_TAP_THR_X, DMP_TAP_THX},
-    {KEY_DMP_TAP_THR_Y, DMP_TAP_THY},
-    {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ},
-    {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y},
-    {KEY_DMP_SH_TH_X, DMP_SH_TH_X},
-    {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z},
-    {KEY_DMP_ORIENT, DMP_ORIENT}
-};
-
-#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-static const unsigned short sConfig = 0x013f;
-#define SCD (1024)
-static const unsigned char dmpMemory[SCD] = {
-    0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x5a, 0xd6, 0x96, 0x06, 0x3f, 0xa3, 0x00, 0x00,
-    0x20, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x77, 0x8e, 0x00, 0x01, 0x00, 0x01,
-    0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01,
-    0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x03, 0x06, 0x00, 0x00, 0x05, 0x01, 0xe9, 0xa2, 0x0f,
-    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
-    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x3e, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
-    0xfc, 0xba, 0xfa, 0x00, 0x01, 0x00, 0x80, 0x00, 0x02, 0x01, 0x80, 0x00, 0x03, 0x02, 0x80, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
-    0x00, 0x7d, 0x32, 0xba, 0x00, 0x0a, 0x1e, 0xd1, 0x00, 0x3a, 0xe8, 0x25, 0x00, 0x00, 0x00, 0x00,
-    0x3f, 0xd7, 0x96, 0x08, 0xff, 0xb3, 0x39, 0xf5, 0xfe, 0x11, 0x1b, 0x62, 0xfb, 0xf4, 0xb4, 0x52,
-    0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc,
-    0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4,
-    0x0d, 0x68, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10,
-
-    0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8,
-    0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x12, 0x82, 0x2d, 0x90,
-    0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0xff, 0xff, 0x00, 0x05, 0x02, 0x00, 0x00, 0x0c,
-    0x00, 0x03, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
-    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, 0x20, 0x00, 0xff, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00,
-    0xff, 0xec, 0x3f, 0xc8, 0xff, 0xee, 0x00, 0x00, 0xff, 0xfe, 0x40, 0x00, 0xff, 0xff, 0xff, 0xc8,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0xff, 0xff, 0xff, 0xff, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x03, 0x65, 0x00, 0x00, 0x00, 0x99, 0x00, 0x00, 0x02, 0xf5,
-
-    0x9e, 0xc5, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 0xc1, 0x83, 0x06, 0x26,
-    0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xc4, 0xad, 0x00, 0x2c, 0x54, 0x7c, 0xf9, 0xc5, 0xa3,
-    0xc1, 0xc3, 0x8f, 0x96, 0x19, 0xa6, 0x81, 0xda, 0x0c, 0xd9, 0x2e, 0xd8, 0xa3, 0x86, 0x31, 0x81,
-    0xa6, 0xd9, 0x30, 0x26, 0xd8, 0xd8, 0xfa, 0xc1, 0x8c, 0xc2, 0x99, 0xc5, 0xa3, 0x2d, 0x55, 0x7d,
-    0x81, 0x91, 0xac, 0x38, 0xad, 0x3a, 0xc3, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9,
-    0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9,
-    0x04, 0xae, 0xd8, 0x79, 0xd9, 0x04, 0xd8, 0x81, 0xfb, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81,
-    0xad, 0xd9, 0x01, 0xd8, 0xfa, 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad,
-    0xad, 0xad, 0xad, 0xfb, 0x2a, 0xd8, 0xd8, 0xf9, 0xc0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xfb,
-    0xac, 0x2e, 0x2e, 0xf9, 0xc1, 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30,
-    0x18, 0xa8, 0x98, 0x81, 0x28, 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xfa,
-    0xc0, 0x89, 0xac, 0x91, 0x2c, 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8,
-    0xd8, 0x79, 0xd9, 0xd8, 0xd8, 0xf9, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9,
-    0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xc1, 0x83, 0x93, 0x35, 0x3d, 0x80,
-    0x25, 0xda, 0xd8, 0xd8, 0x85, 0x69, 0xda, 0xd8, 0xd8, 0xf9, 0xc2, 0x93, 0x81, 0xa3, 0x28, 0x34,
-    0x3c, 0xfb, 0x91, 0xab, 0x8b, 0x18, 0xa3, 0x09, 0xd9, 0xab, 0x97, 0x0a, 0x91, 0x3c, 0xc0, 0x87,
-
-    0x9c, 0xc5, 0xa3, 0xdd, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x95, 0xf9, 0xa3, 0xa3, 0xa3, 0x9d, 0xf9,
-    0xa3, 0xa3, 0xa3, 0xa3, 0xf9, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, 0x91, 0xc3, 0x99, 0xf9, 0xa3, 0xa3,
-    0xa3, 0x98, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xfb, 0x9b, 0xa3, 0xa3,
-    0xdc, 0xc5, 0xa7, 0xf9, 0x26, 0x26, 0x26, 0xd8, 0xd8, 0xff, 0xd8, 0xd8, 0xd8, 0xd8, 0xd8, 0xc1,
-    0xc2, 0xc4, 0x81, 0xa0, 0x90, 0xfa, 0x2c, 0x80, 0x74, 0xfb, 0x70, 0xfa, 0x7c, 0xc0, 0x86, 0x98,
-    0xa8, 0xf9, 0xc9, 0x88, 0xa1, 0xfa, 0x0e, 0x97, 0x80, 0xf9, 0xa9, 0x2e, 0x2e, 0x2e, 0xaa, 0x2e,
-    0x2e, 0x2e, 0xfa, 0xaa, 0xc9, 0x2c, 0xcb, 0xa9, 0x4c, 0xcd, 0x6c, 0xf9, 0x89, 0xa5, 0xca, 0xcd,
-    0xcf, 0xc3, 0x9e, 0xa9, 0x3e, 0x5e, 0x7e, 0x85, 0xa5, 0x1a, 0x3e, 0x5e, 0xc2, 0xa5, 0x99, 0xfb,
-    0x08, 0x34, 0x5c, 0xf9, 0xa9, 0xc9, 0xcb, 0xcd, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97,
-    0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0xa9,
-    0xf9, 0x89, 0x26, 0x46, 0x66, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xaa, 0x98, 0x82, 0x87, 0x2d,
-    0x35, 0x3d, 0xc5, 0xa3, 0xc2, 0xc1, 0x97, 0x80, 0x4a, 0x4e, 0x4e, 0xa3, 0xfa, 0x48, 0xcd, 0xc9,
-    0xf9, 0xc4, 0xa9, 0x99, 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, 0xc3, 0x93,
-    0xa3, 0x0e, 0x16, 0x1e, 0xa9, 0x2c, 0x54, 0x7c, 0xc0, 0xc2, 0x83, 0x97, 0xaf, 0x08, 0xc4, 0xa8,
-    0x11, 0xc1, 0x8f, 0xc5, 0xaf, 0x98, 0xf8, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf9, 0xa3, 0x29,
-    0x55, 0x7d, 0xaf, 0x83, 0xc3, 0x93, 0xaf, 0xf8, 0x00, 0x28, 0x50, 0xc4, 0xc2, 0xc0, 0xf9, 0x97,
-};
-static tKeyLabel keys[NUM_KEYS];
-
-static unsigned short inv_setup_dmpGetAddress(unsigned short key)
-{
-    static int isSorted = 0;
-    if ( !isSorted ) {
-        int kk;
-        for (kk=0; kk<NUM_KEYS; ++kk) {
-            keys[ kk ].addr = 0xffff;
-            keys[ kk ].key = kk;
-        }
-        for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) {
-            keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr;
-        }
-        isSorted = 1;
-    }
-    if ( key >= NUM_KEYS )
-        return 0xffff;
-    return keys[ key ].addr;
-}
-
-
-/**
- *  @brief
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_setup_dmp(void)
-{
-    inv_error_t result;
-    inv_set_get_address( inv_setup_dmpGetAddress );
-
-    result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_full_scale(2000.f);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_load_dmp(dmpMemory, SCD, sConfig);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_ignore_system_suspend(FALSE);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    {
-        struct ext_slave_config config;
-        long odr;
-        config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND;
-        config.len = sizeof(long);
-        config.apply = FALSE;
-        config.data = &odr;
-
-        odr = 0;
-        result = inv_mpu_config_accel(inv_get_dl_config(),
-                                  inv_get_serial_handle(),
-                                  inv_get_serial_handle(),
-                                  &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
-        odr = 200000;
-        result = inv_mpu_config_accel(inv_get_dl_config(), 
-                                  inv_get_serial_handle(),
-                                  inv_get_serial_handle(),
-                                  &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND;
-        odr = MPU_SLAVE_IRQ_TYPE_NONE;
-        result = inv_mpu_config_accel(inv_get_dl_config(),
-                                  inv_get_serial_handle(),
-                                  inv_get_serial_handle(),
-                                  &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
-        odr = MPU_SLAVE_IRQ_TYPE_NONE;
-        result = inv_mpu_config_accel(inv_get_dl_config(), 
-                         inv_get_serial_handle(),
-                         inv_get_serial_handle(),
-                         &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-    }
-
-    return result;
-}
-/**
- * @}
- */
-
diff --git a/mlsdk/mllite/dmpDefault.h b/mlsdk/mllite/dmpDefault.h
deleted file mode 100644
index 0670977..0000000
--- a/mlsdk/mllite/dmpDefault.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 ML_DMPDEFAULT_H__
-#define ML_DMPDEFAULT_H__
-
-/**
- * @defgroup DEFAULT 
- * @brief Default DMP assembly listing header file
- *
- * @{
- *      @file     inv_setup_dmp.c
- *      @brief    Default DMP assembly listing header file
- */
-
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-    inv_error_t inv_setup_dmp(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // ML_DMPDEFAULT_H__
diff --git a/mlsdk/mllite/dmpDefaultMantis.c b/mlsdk/mllite/dmpDefaultMantis.c
deleted file mode 100644
index f35aaca..0000000
--- a/mlsdk/mllite/dmpDefaultMantis.c
+++ /dev/null
@@ -1,467 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/* WARNING: autogenerated code, do not modify */
-/**
- * @defgroup DMPDEFAULT 
- * @brief  
- *
- * @{
- * @file     inv_setup_dmpMantis.c
- * @brief    
- *
- *
- */
-
-#include "mltypes.h"
-#include "dmpDefault.h"
-#include "dmpKey.h"
-#include "dmpmap.h"
-#include "mldl.h"
-
-#define CFG_25  1786
-#define CFG_24  1782
-#define CFG_26  1790
-#define CFG_21  1899
-#define CFG_20  1728
-#define CFG_23  1911
-#define CFG_TAP4  1905
-#define CFG_TAP5  1906
-#define CFG_TAP6  1907
-#define CFG_1  1865
-#define CFG_TAP0  1899
-#define CFG_TAP1  1901
-#define CFG_TAP2  1902
-#define CFG_TAP_SAVE_ACCB  1770
-#define CFG_ORIENT_IRQ_1  1798
-#define CFG_ORIENT_IRQ_2  1821
-#define CFG_ORIENT_IRQ_3  1826
-#define FCFG_MAG_VAL  774
-#define CFG_TAP_QUANTIZE  1730
-#define FCFG_3  936
-#define FCFG_1  891
-#define CFG_ACCEL_FILTER  1076
-#define FCFG_2  895
-#define CFG_3D  1629
-#define FCFG_7  902
-#define CFG_3C  1627
-#define FCFG_5  1030
-#define FCFG_4  880
-#define CFG_3B  1625
-#define CFG_TAP_JERK  1722
-#define FCFG_6  954
-#define CFG_12  1894
-#define CFG_TAP7  1908
-#define CFG_14  1871
-#define CFG_15  1876
-#define CFG_16  1912
-#define CFG_TAP_CLEAR_STICKY  1914
-#define CFG_6  1920
-#define CFG_7  1014
-#define CFG_4  1634
-#define CFG_5  1831
-#define CFG_3  1623
-#define CFG_GYRO_SOURCE  1859
-#define CFG_TAP3  1903
-#define CFG_EXTERNAL  1884
-#define CFG_8  1854
-#define CFG_9  1860
-#define CFG_ORIENT_2  1816
-#define CFG_ORIENT_1  1796
-#define CFG_MOTION_BIAS  1023
-#define FCFG_MAG_MOV  791
-#define TEMPLABEL  1491
-#define FCFG_ACCEL_INIT  847
-
-
-#define D_0_22  (22+512)
-#define D_0_24  (24+512)
-
-#define D_0_36  (36)
-#define D_0_52  (52)
-#define D_0_96  (96)
-#define D_0_104 (104)
-#define D_0_108 (108)
-#define D_0_163 (163)
-#define D_0_188 (188)
-#define D_0_192 (192)
-#define D_0_224 (224)
-#define D_0_228 (228)
-#define D_0_232 (232)
-#define D_0_236 (236)
-
-#define D_1_2 (256 + 2)
-#define D_1_4 (256 + 4)
-#define D_1_8 (256 + 8)
-#define D_1_10 (256 + 10)
-#define D_1_24 (256 + 24)
-#define D_1_28 (256 + 28)
-#define D_1_92 (256 + 92)
-#define D_1_96 (256 + 96)
-#define D_1_98 (256 + 98)
-#define D_1_106 (256 + 106)
-#define D_1_108 (256 + 108)
-#define D_1_112 (256 + 112)
-#define D_1_128 (256 + 144)
-#define D_1_152 (256 + 12)
-#define D_1_168 (256 + 168)
-#define D_1_175 (256 + 175)
-#define D_1_178 (256 + 178)
-#define D_1_236 (256 + 236)
-#define D_1_244 (256 + 244)
-#define D_2_12  (512+12)
-#define D_2_96  (512+96)
-#define D_2_108 (512+108)
-#define D_2_244 (512+244)
-#define D_2_248 (512+248)
-#define D_2_252 (512+252)
-
-#define CPASS_BIAS_X (35*16+4)
-#define CPASS_BIAS_Y (35*16+8)
-#define CPASS_BIAS_Z (35*16+12)
-#define CPASS_MTX_00 (36*16)
-#define CPASS_MTX_01 (36*16+4)
-#define CPASS_MTX_02 (36*16+8)
-#define CPASS_MTX_10 (36*16+12)
-#define CPASS_MTX_11 (37*16)
-#define CPASS_MTX_12 (37*16+4)
-#define CPASS_MTX_20 (37*16+8)
-#define CPASS_MTX_21 (37*16+12)
-#define CPASS_MTX_22 (43*16+12)
-#define D_ACT0    (40*16)
-#define D_ACSX   (40*16+4)
-#define D_ACSY   (40*16+8)
-#define D_ACSZ   (40*16+12)
-
-static const tKeyLabel dmpTConfig[] = {
-    {KEY_CFG_25, CFG_25},
-    {KEY_CFG_24, CFG_24},
-    {KEY_CFG_26, CFG_26},
-    {KEY_CFG_21, CFG_21},
-    {KEY_CFG_20, CFG_20},
-    {KEY_CFG_23, CFG_23},
-    {KEY_CFG_TAP4, CFG_TAP4},
-    {KEY_CFG_TAP5, CFG_TAP5},
-    {KEY_CFG_TAP6, CFG_TAP6},
-    {KEY_CFG_1, CFG_1},
-    {KEY_CFG_TAP0, CFG_TAP0},
-    {KEY_CFG_TAP1, CFG_TAP1},
-    {KEY_CFG_TAP2, CFG_TAP2},
-    {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB},
-    {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1},
-    {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2},
-    {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3},
-    {KEY_FCFG_MAG_VAL, FCFG_MAG_VAL},
-    {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE},
-    {KEY_FCFG_3, FCFG_3},
-    {KEY_FCFG_1, FCFG_1},
-//    {KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER},
-    {KEY_FCFG_2, FCFG_2},
-    {KEY_CFG_3D, CFG_3D},
-    {KEY_FCFG_7, FCFG_7},
-    {KEY_CFG_3C, CFG_3C},
-    {KEY_FCFG_5, FCFG_5},
-    {KEY_FCFG_4, FCFG_4},
-    {KEY_CFG_3B, CFG_3B},
-    {KEY_CFG_TAP_JERK, CFG_TAP_JERK},
-    {KEY_FCFG_6, FCFG_6},
-    {KEY_CFG_12, CFG_12},
-    {KEY_CFG_TAP7, CFG_TAP7},
-    {KEY_CFG_14, CFG_14},
-    {KEY_CFG_15, CFG_15},
-    {KEY_CFG_16, CFG_16},
-    {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY},
-    {KEY_CFG_6, CFG_6},
-    {KEY_CFG_7, CFG_7},
-    {KEY_CFG_4, CFG_4},
-    {KEY_CFG_5, CFG_5},
-    {KEY_CFG_3, CFG_3},
-    {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE},
-    {KEY_CFG_TAP3, CFG_TAP3},
-    {KEY_CFG_EXTERNAL, CFG_EXTERNAL},
-    {KEY_CFG_8, CFG_8},
-    {KEY_CFG_9, CFG_9},
-    {KEY_CFG_ORIENT_2, CFG_ORIENT_2},
-    {KEY_CFG_ORIENT_1, CFG_ORIENT_1},
-    {KEY_CFG_MOTION_BIAS, CFG_MOTION_BIAS},
-    {KEY_FCFG_MAG_MOV, FCFG_MAG_MOV},
-//    {KEY_TEMPLABEL, TEMPLABEL},
-    {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT},
-    {KEY_D_0_22, D_0_22},
-    {KEY_D_0_24, D_0_24},
-    {KEY_D_0_36, D_0_36},
-    {KEY_D_0_52, D_0_52},
-    {KEY_D_0_96, D_0_96},
-    {KEY_D_0_104, D_0_104},
-    {KEY_D_0_108, D_0_108},
-    {KEY_D_0_163, D_0_163},
-    {KEY_D_0_188, D_0_188},
-    {KEY_D_0_192, D_0_192},
-    {KEY_D_0_224, D_0_224},
-    {KEY_D_0_228, D_0_228},
-    {KEY_D_0_232, D_0_232},
-    {KEY_D_0_236, D_0_236},
-
-    {KEY_DMP_PREVPTAT, DMP_PREVPTAT},
-    {KEY_D_1_2, D_1_2},
-    {KEY_D_1_4, D_1_4},
-    {KEY_D_1_8, D_1_8},
-    {KEY_D_1_10, D_1_10},
-    {KEY_D_1_24, D_1_24},
-    {KEY_D_1_28, D_1_28},
-    {KEY_D_1_92, D_1_92},
-    {KEY_D_1_96, D_1_96},
-    {KEY_D_1_98, D_1_98},
-    {KEY_D_1_106, D_1_106},
-    {KEY_D_1_108, D_1_108},
-    {KEY_D_1_112, D_1_112},
-    {KEY_D_1_128, D_1_128},
-    {KEY_D_1_152, D_1_152},
-    {KEY_D_1_168, D_1_168},
-    {KEY_D_1_175, D_1_175},
-    {KEY_D_1_178, D_1_178},
-    {KEY_D_1_236, D_1_236},
-    {KEY_D_1_244, D_1_244},
-
-    {KEY_D_2_12,  D_2_12},
-    {KEY_D_2_96,  D_2_96},
-    {KEY_D_2_108, D_2_108},
-    {KEY_D_2_244, D_2_244},
-    {KEY_D_2_248, D_2_248},
-    {KEY_D_2_252, D_2_252},
-
-    {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN},
-    {KEY_DMP_TAP_THR_X, DMP_TAP_THX},
-    {KEY_DMP_TAP_THR_Y, DMP_TAP_THY},
-    {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ},
-    {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y},
-    {KEY_DMP_SH_TH_X, DMP_SH_TH_X},
-    {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z},
-    {KEY_DMP_ORIENT, DMP_ORIENT},
-
-    {KEY_CPASS_BIAS_X, CPASS_BIAS_X},
-    {KEY_CPASS_BIAS_Y, CPASS_BIAS_Y},
-    {KEY_CPASS_BIAS_Z, CPASS_BIAS_Z},
-    {KEY_CPASS_MTX_00, CPASS_MTX_00},
-    {KEY_CPASS_MTX_01, CPASS_MTX_01},
-    {KEY_CPASS_MTX_02, CPASS_MTX_02},
-    {KEY_CPASS_MTX_10, CPASS_MTX_10},
-    {KEY_CPASS_MTX_11, CPASS_MTX_11},
-    {KEY_CPASS_MTX_12, CPASS_MTX_12},
-    {KEY_CPASS_MTX_20, CPASS_MTX_20},
-    {KEY_CPASS_MTX_21, CPASS_MTX_21},
-    {KEY_CPASS_MTX_22, CPASS_MTX_22},
-    {KEY_D_ACT0, D_ACT0},
-    {KEY_D_ACSX, D_ACSX},
-    {KEY_D_ACSY, D_ACSY},
-    {KEY_D_ACSZ, D_ACSZ}
-};
-#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-
-#define DMP_CODE_SIZE 1923
-
-static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
-    0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 
-    0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 
-    0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, 
-    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 
-    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 
-    0x41, 0xff, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x2a, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 
-    0xfd, 0x87, 0x26, 0x50, 0xfd, 0x80, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 
-    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6f, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5e, 0xc0, 
-    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, 
-    0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, 
-    0xff, 0xe6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, 
-    0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, 
-    0xff, 0xd0, 0x9a, 0xbe, 0x00, 0x00, 0x10, 0xa9, 0xff, 0xf4, 0x1e, 0xb2, 0x00, 0xce, 0xbb, 0xf7, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0c, 
-    0xff, 0xc2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xcf, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 
-    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, 0x03, 0x3f, 0x68, 0xb6, 0x79, 0x35, 0x28, 0xbc, 0xc6, 0x7e, 0xd1, 0x6c, 
-    0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 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, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, 
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xff, 0xef, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 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, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 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, 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, 
-
-    0xd8, 0xdc, 0xba, 0xa2, 0xf1, 0xde, 0xb2, 0xb8, 0xb4, 0xa8, 0x81, 0x91, 0xf7, 0x4a, 0x90, 0x7f, 
-    0x91, 0x6a, 0xf3, 0xf9, 0xdb, 0xa8, 0xf9, 0xb0, 0xba, 0xa0, 0x80, 0xf2, 0xce, 0x81, 0xf3, 0xc2, 
-    0xf1, 0xc1, 0xf2, 0xc3, 0xf3, 0xcc, 0xa2, 0xb2, 0x80, 0xf1, 0xc6, 0xd8, 0x80, 0xba, 0xa7, 0xdf, 
-    0xdf, 0xdf, 0xf2, 0xa7, 0xc3, 0xcb, 0xc5, 0xb6, 0xf0, 0x87, 0xa2, 0x94, 0x24, 0x48, 0x70, 0x3c, 
-    0x95, 0x40, 0x68, 0x34, 0x58, 0x9b, 0x78, 0xa2, 0xf1, 0x83, 0x92, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 
-    0xb4, 0xb8, 0xa1, 0xd0, 0x91, 0x80, 0xf2, 0x70, 0xf3, 0x70, 0xf2, 0x7c, 0x80, 0xa8, 0xf1, 0x01, 
-    0xb0, 0x98, 0x87, 0xd9, 0x43, 0xd8, 0x86, 0xc9, 0x88, 0xba, 0xa1, 0xf2, 0x0e, 0xb8, 0x97, 0x80, 
-    0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0xc1, 
-    0xc9, 0xc3, 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, 0xb9, 0xaf, 0xb4, 0xb0, 
-    0x83, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb1, 0x8f, 0x98, 0xb9, 0xaf, 0xf0, 0x24, 0x08, 0x44, 0x10, 
-    0x64, 0x18, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xaf, 0x83, 0xb5, 0x93, 0xaf, 0xf0, 0x00, 0x28, 0x50, 
-    0xf1, 0xa3, 0x86, 0x9f, 0x61, 0xa6, 0xda, 0xde, 0xdf, 0xd9, 0xfa, 0xa3, 0x86, 0x96, 0xdb, 0x31, 
-    0xa6, 0xd9, 0xf8, 0xdf, 0xba, 0xa6, 0x8f, 0xc2, 0xc5, 0xc7, 0xb2, 0x8c, 0xc1, 0xb8, 0xa2, 0xdf, 
-    0xdf, 0xdf, 0xa3, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb2, 0x86, 0xb4, 0x98, 0x0d, 
-    0x35, 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 
-    0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xb9, 
-    0xa3, 0xde, 0xdf, 0xdf, 0xa3, 0xb1, 0x80, 0xf2, 0xc4, 0xcd, 0xc9, 0xf1, 0xb8, 0xa9, 0xb4, 0x99, 
-    0x83, 0x0d, 0x35, 0x5d, 0x89, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0xb5, 0x93, 0xa3, 0x0e, 0x16, 0x1e, 
-    0xa9, 0x2c, 0x54, 0x7c, 0xb8, 0xb4, 0xb0, 0xf1, 0x97, 0x83, 0xa8, 0x11, 0x84, 0xa5, 0x09, 0x98, 
-    0xa3, 0x83, 0xf0, 0xda, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xd8, 0xf1, 0xa5, 0x29, 0x55, 0x7d, 
-    0xa5, 0x85, 0x95, 0x02, 0x1a, 0x2e, 0x3a, 0x56, 0x5a, 0x40, 0x48, 0xf9, 0xf3, 0xa3, 0xd9, 0xf8, 
-    0xf0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xa8, 0xf1, 0x11, 0xf0, 0x98, 
-    0xa2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xda, 0xf3, 0xde, 0xd8, 0x83, 0xa5, 0x94, 0x01, 0xd9, 
-    0xa3, 0x02, 0xf1, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, 0xf1, 0x84, 0x92, 0xa2, 0x4d, 0xda, 0x2a, 0xd8, 
-    0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, 
-    0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0x93, 0xa3, 0x4d, 0xda, 0x2a, 0xd8, 
-    0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, 
-    0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0xa8, 0x8a, 0x9a, 0xf0, 0x28, 0x50, 
-    0x78, 0x9e, 0xf3, 0x88, 0x18, 0xf1, 0x9f, 0x1d, 0x98, 0xa8, 0xd9, 0x08, 0xd8, 0xc8, 0x9f, 0x12, 
-    0x9e, 0xf3, 0x15, 0xa8, 0xda, 0x12, 0x10, 0xd8, 0xf1, 0xaf, 0xc8, 0x97, 0x87, 0x34, 0xb5, 0xb9, 
-    0x94, 0xa4, 0x21, 0xf3, 0xd9, 0x22, 0xd8, 0xf2, 0x2d, 0xf3, 0xd9, 0x2a, 0xd8, 0xf2, 0x35, 0xf3, 
-    0xd9, 0x32, 0xd8, 0x81, 0xa4, 0x60, 0x60, 0x61, 0xd9, 0x61, 0xd8, 0x6c, 0x68, 0x69, 0xd9, 0x69, 
-    0xd8, 0x74, 0x70, 0x71, 0xd9, 0x71, 0xd8, 0xb1, 0xa3, 0x84, 0x19, 0x3d, 0x5d, 0xa3, 0x83, 0x1a, 
-    0x3e, 0x5e, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xb8, 0xb0, 0xaf, 0x8f, 0x94, 0xf2, 0xda, 0x3e, 
-    0xd8, 0xb4, 0x9a, 0xa8, 0x87, 0x29, 0xda, 0xf8, 0xd8, 0x87, 0x9a, 0x35, 0xda, 0xf8, 0xd8, 0x87, 
-    0x9a, 0x3d, 0xda, 0xf8, 0xd8, 0xb1, 0xb9, 0xa4, 0x98, 0x85, 0x02, 0x2e, 0x56, 0xa5, 0x81, 0x00, 
-    0x0c, 0x14, 0xa3, 0x97, 0xb0, 0x8a, 0xf1, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 
-    0xd9, 0x68, 0xd8, 0xb1, 0x84, 0x0d, 0xda, 0x0e, 0xd8, 0xa3, 0x29, 0x83, 0xda, 0x2c, 0x0e, 0xd8, 
-    0xa3, 0x84, 0x49, 0x83, 0xda, 0x2c, 0x4c, 0x0e, 0xd8, 0xb8, 0xb0, 0xa8, 0x8a, 0x9a, 0xf5, 0x20, 
-    0xaa, 0xda, 0xdf, 0xd8, 0xa8, 0x40, 0xaa, 0xd0, 0xda, 0xde, 0xd8, 0xa8, 0x60, 0xaa, 0xda, 0xd0, 
-    0xdf, 0xd8, 0xf1, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 
-    0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb8, 0xb0, 0xb4, 0xa8, 0x8c, 0x9c, 0xf0, 0x04, 0x28, 0x51, 0x79, 
-    0x1d, 0x30, 0x14, 0x38, 0xb2, 0x82, 0xab, 0xd0, 0x98, 0x2c, 0x50, 0x50, 0x78, 0x78, 0x9b, 0xf1, 
-    0x1a, 0xb0, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 
-    0x59, 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 
-    0x31, 0x8b, 0x30, 0x49, 0x60, 0xa5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 
-    0x49, 0x30, 0x19, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, 
-    0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, 
-    0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, 
-    0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, 
-    0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, 
-    0x88, 0x2d, 0x55, 0x7d, 0x9e, 0xb9, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 
-    0xb1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xb8, 0xad, 0x00, 0x2c, 0x54, 
-    0x7c, 0xf2, 0xb1, 0x8c, 0xb4, 0x99, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0x81, 0x91, 0xac, 0x38, 0xad, 
-    0x3a, 0xb5, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 
-    0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, 0x04, 0xae, 0xd8, 0x79, 0xd9, 
-    0x04, 0xd8, 0x81, 0xf3, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, 0xad, 0xd9, 0x01, 0xd8, 0xf2, 
-    0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, 0xad, 0xad, 0xad, 0xf3, 0x2a, 
-    0xd8, 0xd8, 0xf1, 0xb0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xf3, 0xac, 0x2e, 0x2e, 0xf1, 0xb1, 
-    0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, 0x18, 0xa8, 0x98, 0x81, 0x28, 
-    0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xf2, 0xb0, 0x89, 0xac, 0x91, 0x2c, 
-    0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, 0xd8, 0x79, 0xd9, 0xd8, 0xd8, 
-    0xf1, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 
-    0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xb1, 0x83, 0x93, 0x35, 0x3d, 0x80, 0x25, 0xda, 0xd8, 0xd8, 0x85, 
-    0x69, 0xda, 0xd8, 0xd8, 0xb4, 0x93, 0x81, 0xa3, 0x28, 0x34, 0x3c, 0xf3, 0xab, 0x8b, 0xf8, 0xa3, 
-    0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xfa, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xa3, 
-    0xa3, 0xa3, 0xa3, 0x95, 0xf1, 0xa3, 0xa3, 0xa3, 0x9d, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, 0xf2, 0xa3, 
-    0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xb2, 0xa3, 0xa3, 0xa3, 0xa3, 
-    0xa3, 0xa3, 0xb0, 0x87, 0xb5, 0x99, 0xf1, 0xa3, 0xa3, 0xa3, 0x98, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, 
-    0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xb9, 0xa7, 0xf1, 0x26, 0x26, 0x26, 
-    0xd8, 0xd8, 0xff
-};
-
-static unsigned short sStartAddress = 0x0300;
-
-
-static tKeyLabel keys[NUM_KEYS];
-
-static unsigned short inv_setup_dmpGetAddress( unsigned short key )
-{
-    static int isSorted = 0;
-    if ( !isSorted ) {
-        int kk;
-        for (kk=0; kk<NUM_KEYS; ++kk) {
-            keys[ kk ].addr = 0xffff;
-            keys[ kk ].key = kk;
-        }
-        for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) {
-            keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr;
-        }
-        isSorted = 1;
-    }
-    if ( key >= NUM_KEYS )
-        return 0xffff;
-    return keys[ key ].addr;
-}
-
-/**
- *  @brief
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_setup_dmp(void)
-
-{
-    inv_error_t result;
-
-    inv_set_get_address(inv_setup_dmpGetAddress);
-    result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_full_scale(2000.f);  
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sStartAddress);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_external_sync(MPU_EXT_SYNC_TEMP);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-/**
- *@}
- */
diff --git a/mlsdk/mllite/dmpKey.h b/mlsdk/mllite/dmpKey.h
deleted file mode 100644
index f152644..0000000
--- a/mlsdk/mllite/dmpKey.h
+++ /dev/null
@@ -1,432 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 DMPKEY_H__
-#define DMPKEY_H__
-
-#define KEY_CFG_25  0
-#define KEY_CFG_24                 (KEY_CFG_25+1)
-#define KEY_CFG_26                 (KEY_CFG_24+1)
-#define KEY_CFG_21                 (KEY_CFG_26+1)
-#define KEY_CFG_20                 (KEY_CFG_21+1)
-#define KEY_CFG_TAP4               (KEY_CFG_20+1)
-#define KEY_CFG_TAP5               (KEY_CFG_TAP4+1)
-#define KEY_CFG_TAP6               (KEY_CFG_TAP5+1)
-#define KEY_CFG_TAP7               (KEY_CFG_TAP6+1)
-#define KEY_CFG_TAP0               (KEY_CFG_TAP7+1)
-#define KEY_CFG_TAP1               (KEY_CFG_TAP0+1)
-#define KEY_CFG_TAP2               (KEY_CFG_TAP1+1)
-#define KEY_CFG_TAP3               (KEY_CFG_TAP2+1)
-#define KEY_CFG_TAP_QUANTIZE       (KEY_CFG_TAP3+1)
-#define KEY_CFG_TAP_JERK           (KEY_CFG_TAP_QUANTIZE+1)
-#define KEY_CFG_TAP_SAVE_ACCB      (KEY_CFG_TAP_JERK+1)
-#define KEY_CFG_TAP_CLEAR_STICKY   (KEY_CFG_TAP_SAVE_ACCB+1)
-#define KEY_FCFG_ACCEL_INPUT       (KEY_CFG_TAP_CLEAR_STICKY +1)
-#define KEY_FCFG_ACCEL_INIT        (KEY_FCFG_ACCEL_INPUT+1)
-#define KEY_CFG_23                 (KEY_FCFG_ACCEL_INIT+1)
-#define KEY_FCFG_1                 (KEY_CFG_23+1)
-#define KEY_FCFG_3                 (KEY_FCFG_1+1)
-#define KEY_FCFG_2                 (KEY_FCFG_3+1)
-#define KEY_CFG_3D                 (KEY_FCFG_2+1)
-#define KEY_CFG_3B                 (KEY_CFG_3D+1)
-#define KEY_CFG_3C                 (KEY_CFG_3B+1)
-#define KEY_FCFG_5                 (KEY_CFG_3C+1)
-#define KEY_FCFG_4                 (KEY_FCFG_5+1)
-#define KEY_FCFG_7                 (KEY_FCFG_4+1)
-#define KEY_FCFG_FSCALE            (KEY_FCFG_7+1)
-#define KEY_FCFG_AZ                (KEY_FCFG_FSCALE+1)
-#define KEY_FCFG_6                 (KEY_FCFG_AZ+1)
-#define KEY_FCFG_LSB4              (KEY_FCFG_6+1)
-#define KEY_CFG_12                 (KEY_FCFG_LSB4+1)
-#define KEY_CFG_14                 (KEY_CFG_12+1)
-#define KEY_CFG_15                 (KEY_CFG_14+1)
-#define KEY_CFG_16                 (KEY_CFG_15+1)
-#define KEY_CFG_18                 (KEY_CFG_16+1)
-#define KEY_CFG_6                  (KEY_CFG_18 + 1)
-#define KEY_CFG_7                  (KEY_CFG_6+1)
-#define KEY_CFG_4                  (KEY_CFG_7+1)
-#define KEY_CFG_5                  (KEY_CFG_4+1)
-#define KEY_CFG_2                  (KEY_CFG_5+1)
-#define KEY_CFG_3                  (KEY_CFG_2+1)
-#define KEY_CFG_1                  (KEY_CFG_3+1)
-#define KEY_CFG_EXTERNAL           (KEY_CFG_1+1)
-#define KEY_CFG_8                  (KEY_CFG_EXTERNAL+1)
-#define KEY_CFG_9                  (KEY_CFG_8+1)
-#define KEY_CFG_ORIENT_3           (KEY_CFG_9 + 1)
-#define KEY_CFG_ORIENT_2           (KEY_CFG_ORIENT_3 + 1)
-#define KEY_CFG_ORIENT_1           (KEY_CFG_ORIENT_2 + 1)
-#define KEY_CFG_GYRO_SOURCE        (KEY_CFG_ORIENT_1 + 1)
-#define KEY_CFG_ORIENT_IRQ_1       (KEY_CFG_GYRO_SOURCE + 1)
-#define KEY_CFG_ORIENT_IRQ_2       (KEY_CFG_ORIENT_IRQ_1 + 1)
-#define KEY_CFG_ORIENT_IRQ_3       (KEY_CFG_ORIENT_IRQ_2 + 1)
-#define KEY_FCFG_MAG_VAL           (KEY_CFG_ORIENT_IRQ_3 + 1)
-#define KEY_FCFG_MAG_MOV           (KEY_FCFG_MAG_VAL + 1)
-
-#define KEY_D_0_22                 (KEY_FCFG_MAG_MOV + 1)
-#define KEY_D_0_24                 (KEY_D_0_22+1)
-#define KEY_D_0_36                 (KEY_D_0_24+1)
-#define KEY_D_0_52                 (KEY_D_0_36+1)
-#define KEY_D_0_96                 (KEY_D_0_52+1)
-#define KEY_D_0_104                (KEY_D_0_96+1)
-#define KEY_D_0_108                (KEY_D_0_104+1)
-#define KEY_D_0_163                (KEY_D_0_108+1)
-#define KEY_D_0_188                (KEY_D_0_163+1)
-#define KEY_D_0_192                (KEY_D_0_188+1)
-#define KEY_D_0_224                (KEY_D_0_192+1)
-#define KEY_D_0_228                (KEY_D_0_224+1)
-#define KEY_D_0_232                (KEY_D_0_228+1)
-#define KEY_D_0_236                (KEY_D_0_232+1)
-
-#define KEY_DMP_PREVPTAT           (KEY_D_0_236+1)
-#define KEY_D_1_2                  (KEY_DMP_PREVPTAT+1)
-#define KEY_D_1_4                  (KEY_D_1_2 + 1)
-#define KEY_D_1_8                  (KEY_D_1_4 + 1)
-#define KEY_D_1_10                 (KEY_D_1_8+1)
-#define KEY_D_1_24                 (KEY_D_1_10+1)
-#define KEY_D_1_28                 (KEY_D_1_24+1)
-#define KEY_D_1_92                 (KEY_D_1_28+1)
-#define KEY_D_1_96                 (KEY_D_1_92+1)
-#define KEY_D_1_98                 (KEY_D_1_96+1)
-#define KEY_D_1_106                (KEY_D_1_98+1)
-#define KEY_D_1_108                (KEY_D_1_106+1)
-#define KEY_D_1_112                (KEY_D_1_108+1)
-#define KEY_D_1_128                (KEY_D_1_112+1)
-#define KEY_D_1_152                (KEY_D_1_128+1)
-#define KEY_D_1_168                (KEY_D_1_152+1)
-#define KEY_D_1_175                (KEY_D_1_168+1)
-#define KEY_D_1_178                (KEY_D_1_175+1)
-#define KEY_D_1_179                (KEY_D_1_178+1)
-#define KEY_D_1_236                (KEY_D_1_179+1)
-#define KEY_D_1_244                (KEY_D_1_236+1)
-#define KEY_D_2_12                 (KEY_D_1_244+1)
-#define KEY_D_2_96                 (KEY_D_2_12+1)
-#define KEY_D_2_108                (KEY_D_2_96+1)
-#define KEY_D_2_244                (KEY_D_2_108+1)
-#define KEY_D_2_248                (KEY_D_2_244+1)
-#define KEY_D_2_252                (KEY_D_2_248+1)
-
-// Compass Keys
-#define KEY_CPASS_BIAS_X            (KEY_D_2_252+1)
-#define KEY_CPASS_BIAS_Y            (KEY_CPASS_BIAS_X+1)
-#define KEY_CPASS_BIAS_Z            (KEY_CPASS_BIAS_Y+1)
-#define KEY_CPASS_MTX_00            (KEY_CPASS_BIAS_Z+1)
-#define KEY_CPASS_MTX_01            (KEY_CPASS_MTX_00+1)
-#define KEY_CPASS_MTX_02            (KEY_CPASS_MTX_01+1)
-#define KEY_CPASS_MTX_10            (KEY_CPASS_MTX_02+1)
-#define KEY_CPASS_MTX_11            (KEY_CPASS_MTX_10+1)
-#define KEY_CPASS_MTX_12            (KEY_CPASS_MTX_11+1)
-#define KEY_CPASS_MTX_20            (KEY_CPASS_MTX_12+1)
-#define KEY_CPASS_MTX_21            (KEY_CPASS_MTX_20+1)
-#define KEY_CPASS_MTX_22            (KEY_CPASS_MTX_21+1)
-
-// Mantis Keys
-#define KEY_CFG_MOTION_BIAS         (KEY_CPASS_MTX_22+1)
-
-#define KEY_DMP_TAPW_MIN           (KEY_CFG_MOTION_BIAS+1)
-#define KEY_DMP_TAP_THR_X          (KEY_DMP_TAPW_MIN+1)
-#define KEY_DMP_TAP_THR_Y          (KEY_DMP_TAP_THR_X+1)
-#define KEY_DMP_TAP_THR_Z          (KEY_DMP_TAP_THR_Y+1)
-#define KEY_DMP_SH_TH_Y            (KEY_DMP_TAP_THR_Z+1)
-#define KEY_DMP_SH_TH_X            (KEY_DMP_SH_TH_Y+1)
-#define KEY_DMP_SH_TH_Z            (KEY_DMP_SH_TH_X+1)
-#define KEY_DMP_ORIENT             (KEY_DMP_SH_TH_Z+1)
-#define KEY_D_ACT0                 (KEY_DMP_ORIENT+1)
-#define KEY_D_ACSX                 (KEY_D_ACT0+1)
-#define KEY_D_ACSY                 (KEY_D_ACSX+1)
-#define KEY_D_ACSZ                 (KEY_D_ACSY+1)
-
-// Pedometer Standalone only keys
-#define KEY_D_PEDSTD_BP_B          (KEY_D_ACSZ+1)
-#define KEY_D_PEDSTD_HP_A          (KEY_D_PEDSTD_BP_B+1)
-#define KEY_D_PEDSTD_HP_B          (KEY_D_PEDSTD_HP_A+1)
-#define KEY_D_PEDSTD_BP_A4         (KEY_D_PEDSTD_HP_B+1)
-#define KEY_D_PEDSTD_BP_A3         (KEY_D_PEDSTD_BP_A4+1)
-#define KEY_D_PEDSTD_BP_A2         (KEY_D_PEDSTD_BP_A3+1)
-#define KEY_D_PEDSTD_BP_A1         (KEY_D_PEDSTD_BP_A2+1)
-#define KEY_D_PEDSTD_INT_THRSH     (KEY_D_PEDSTD_BP_A1+1)
-#define KEY_D_PEDSTD_CLIP          (KEY_D_PEDSTD_INT_THRSH+1)
-#define KEY_D_PEDSTD_SB            (KEY_D_PEDSTD_CLIP+1)
-#define KEY_D_PEDSTD_SB_TIME       (KEY_D_PEDSTD_SB+1)
-#define KEY_D_PEDSTD_PEAKTHRSH     (KEY_D_PEDSTD_SB_TIME+1)
-#define KEY_D_PEDSTD_TIML          (KEY_D_PEDSTD_PEAKTHRSH+1)
-#define KEY_D_PEDSTD_TIMH          (KEY_D_PEDSTD_TIML+1)
-#define KEY_D_PEDSTD_PEAK          (KEY_D_PEDSTD_TIMH+1)
-#define KEY_D_PEDSTD_TIMECTR       (KEY_D_PEDSTD_PEAK+1)
-#define KEY_D_PEDSTD_STEPCTR       (KEY_D_PEDSTD_TIMECTR+1)
-#define KEY_D_PEDSTD_WALKTIME      (KEY_D_PEDSTD_STEPCTR+1)
-
-// EIS Keys
-#define KEY_P_EIS_FIFO_FOOTER      (KEY_D_PEDSTD_WALKTIME+1)
-#define KEY_P_EIS_FIFO_YSHIFT      (KEY_P_EIS_FIFO_FOOTER+1)
-#define KEY_P_EIS_DATA_RATE        (KEY_P_EIS_FIFO_YSHIFT+1)
-#define KEY_P_EIS_FIFO_XSHIFT      (KEY_P_EIS_DATA_RATE+1)
-#define KEY_P_EIS_FIFO_SYNC        (KEY_P_EIS_FIFO_XSHIFT+1)
-#define KEY_P_EIS_FIFO_ZSHIFT      (KEY_P_EIS_FIFO_SYNC+1)
-#define KEY_P_EIS_FIFO_READY       (KEY_P_EIS_FIFO_ZSHIFT+1)
-#define KEY_DMP_FOOTER             (KEY_P_EIS_FIFO_READY+1)
-#define KEY_DMP_INTX_HC            (KEY_DMP_FOOTER+1)
-#define KEY_DMP_INTX_PH            (KEY_DMP_INTX_HC+1)
-#define KEY_DMP_INTX_SH            (KEY_DMP_INTX_PH+1)
-#define KEY_DMP_AINV_SH            (KEY_DMP_INTX_SH +1)
-#define KEY_DMP_A_INV_XH           (KEY_DMP_AINV_SH+1)
-#define KEY_DMP_AINV_PH            (KEY_DMP_A_INV_XH+1)
-#define KEY_DMP_CTHX_H             (KEY_DMP_AINV_PH+1)
-#define KEY_DMP_CTHY_H             (KEY_DMP_CTHX_H+1)
-#define KEY_DMP_CTHZ_H             (KEY_DMP_CTHY_H+1)
-#define KEY_DMP_NCTHX_H            (KEY_DMP_CTHZ_H+1)
-#define KEY_DMP_NCTHY_H            (KEY_DMP_NCTHX_H+1)
-#define KEY_DMP_NCTHZ_H            (KEY_DMP_NCTHY_H+1)
-#define KEY_DMP_CTSQ_XH            (KEY_DMP_NCTHZ_H+1)
-#define KEY_DMP_CTSQ_YH            (KEY_DMP_CTSQ_XH+1)
-#define KEY_DMP_CTSQ_ZH            (KEY_DMP_CTSQ_YH+1)
-#define KEY_DMP_INTX_H             (KEY_DMP_CTSQ_ZH+1)
-#define KEY_DMP_INTY_H             (KEY_DMP_INTX_H+1)
-#define KEY_DMP_INTZ_H             (KEY_DMP_INTY_H+1)
-#define KEY_DMP_HPX_H              (KEY_DMP_INTZ_H+1)
-#define KEY_DMP_HPY_H              (KEY_DMP_HPX_H+1)
-#define KEY_DMP_HPZ_H              (KEY_DMP_HPY_H+1)
-
-// Stream Keys
-#define KEY_STREAM_P_GYRO_Z        (KEY_DMP_HPZ_H + 1)
-#define KEY_STREAM_P_GYRO_Y        (KEY_STREAM_P_GYRO_Z+1)
-#define KEY_STREAM_P_GYRO_X        (KEY_STREAM_P_GYRO_Y+1)
-#define KEY_STREAM_P_TEMP          (KEY_STREAM_P_GYRO_X+1)
-#define KEY_STREAM_P_AUX_Y         (KEY_STREAM_P_TEMP+1)
-#define KEY_STREAM_P_AUX_X         (KEY_STREAM_P_AUX_Y+1)
-#define KEY_STREAM_P_AUX_Z         (KEY_STREAM_P_AUX_X+1)
-#define KEY_STREAM_P_ACCEL_Y       (KEY_STREAM_P_AUX_Z+1)
-#define KEY_STREAM_P_ACCEL_X       (KEY_STREAM_P_ACCEL_Y+1)
-#define KEY_STREAM_P_FOOTER        (KEY_STREAM_P_ACCEL_X+1)
-#define KEY_STREAM_P_ACCEL_Z       (KEY_STREAM_P_FOOTER+1)
-
-#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z+1)
-
-    typedef struct {
-        unsigned short key;
-        unsigned short addr;
-    } tKeyLabel;
-    
-#define DINA0A 0x0a
-#define DINA22 0x22
-#define DINA42 0x42
-#define DINA5A 0x5a
-
-#define DINA06 0x06
-#define DINA0E 0x0e
-#define DINA16 0x16
-#define DINA1E 0x1e
-#define DINA26 0x26
-#define DINA2E 0x2e
-#define DINA36 0x36
-#define DINA3E 0x3e
-#define DINA46 0x46
-#define DINA4E 0x4e
-#define DINA56 0x56
-#define DINA5E 0x5e
-#define DINA66 0x66
-#define DINA6E 0x6e
-#define DINA76 0x76
-#define DINA7E 0x7e
-
-#define DINA00 0x00
-#define DINA08 0x08
-#define DINA10 0x10
-#define DINA18 0x18
-#define DINA20 0x20
-#define DINA28 0x28
-#define DINA30 0x30
-#define DINA38 0x38
-#define DINA40 0x40
-#define DINA48 0x48
-#define DINA50 0x50
-#define DINA58 0x58
-#define DINA60 0x60
-#define DINA68 0x68
-#define DINA70 0x70
-#define DINA78 0x78
-
-#define DINA04 0x04
-#define DINA0C 0x0c
-#define DINA14 0x14
-#define DINA1C 0x1C
-#define DINA24 0x24
-#define DINA2C 0x2c
-#define DINA34 0x34
-#define DINA3C 0x3c
-#define DINA44 0x44
-#define DINA4C 0x4c
-#define DINA54 0x54
-#define DINA5C 0x5c
-#define DINA64 0x64
-#define DINA6C 0x6c
-#define DINA74 0x74
-#define DINA7C 0x7c
-
-#define DINA01 0x01
-#define DINA09 0x09
-#define DINA11 0x11
-#define DINA19 0x19
-#define DINA21 0x21
-#define DINA29 0x29
-#define DINA31 0x31
-#define DINA39 0x39
-#define DINA41 0x41
-#define DINA49 0x49
-#define DINA51 0x51
-#define DINA59 0x59
-#define DINA61 0x61
-#define DINA69 0x69
-#define DINA71 0x71
-#define DINA79 0x79
-
-#define DINA25 0x25
-#define DINA2D 0x2d
-#define DINA35 0x35
-#define DINA3D 0x3d
-#define DINA4D 0x4d
-#define DINA55 0x55
-#define DINA5D 0x5D
-#define DINA6D 0x6d
-#define DINA75 0x75
-#define DINA7D 0x7d
-
-#define DINC00 0x00
-#define DINC01 0x01
-#define DINC02 0x02
-#define DINC03 0x03
-#define DINC08 0x08
-#define DINC09 0x09
-#define DINC0A 0x0a
-#define DINC0B 0x0b
-#define DINC10 0x10
-#define DINC11 0x11
-#define DINC12 0x12
-#define DINC13 0x13
-#define DINC18 0x18
-#define DINC19 0x19
-#define DINC1A 0x1a
-#define DINC1B 0x1b
-
-#define DINC20 0x20
-#define DINC21 0x21
-#define DINC22 0x22
-#define DINC23 0x23
-#define DINC28 0x28
-#define DINC29 0x29
-#define DINC2A 0x2a
-#define DINC2B 0x2b
-#define DINC30 0x30
-#define DINC31 0x31
-#define DINC32 0x32
-#define DINC33 0x33
-#define DINC38 0x38
-#define DINC39 0x39
-#define DINC3A 0x3a
-#define DINC3B 0x3b
-
-#define DINC40 0x40
-#define DINC41 0x41
-#define DINC42 0x42
-#define DINC43 0x43
-#define DINC48 0x48
-#define DINC49 0x49
-#define DINC4A 0x4a
-#define DINC4B 0x4b
-#define DINC50 0x50
-#define DINC51 0x51
-#define DINC52 0x52
-#define DINC53 0x53
-#define DINC58 0x58
-#define DINC59 0x59
-#define DINC5A 0x5a
-#define DINC5B 0x5b
-
-#define DINC60 0x60
-#define DINC61 0x61
-#define DINC62 0x62
-#define DINC63 0x63
-#define DINC68 0x68
-#define DINC69 0x69
-#define DINC6A 0x6a
-#define DINC6B 0x6b
-#define DINC70 0x70
-#define DINC71 0x71
-#define DINC72 0x72
-#define DINC73 0x73
-#define DINC78 0x78
-#define DINC79 0x79
-#define DINC7A 0x7a
-#define DINC7B 0x7b
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
-#define DINA80 0x80
-#define DINA90 0x90
-#define DINAA0 0xa0
-#define DINAC9 0xc9
-#define DINACB 0xcb
-#define DINACD 0xcd
-#define DINACF 0xcf
-#define DINAC8 0xc8
-#define DINACA 0xca
-#define DINACC 0xcc
-#define DINACE 0xce
-#define DINAD8 0xd8
-#define DINADD 0xdd
-#define DINAF8 0xf8
-#define DINAFE 0xfe
-#define DINAC0 0xc0
-#define DINAC1 0xc1
-#define DINAC2 0xc2
-#define DINAC3 0xc3
-#define DINAC4 0xc4
-#define DINAC5 0xc5
-#elif defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-
-#define DINA80 0x80
-#define DINA90 0x90
-#define DINAA0 0xa0
-#define DINAC9 0xc9
-#define DINACB 0xcb
-#define DINACD 0xcd
-#define DINACF 0xcf
-#define DINAC8 0xc8
-#define DINACA 0xca
-#define DINACC 0xcc
-#define DINACE 0xce
-#define DINAD8 0xd8
-#define DINADD 0xdd
-#define DINAF8 0xf0
-#define DINAFE 0xfe
-
-#define DINBF8 0xf8
-#define DINAC0 0xb0
-#define DINAC1 0xb1
-#define DINAC2 0xb4
-#define DINAC3 0xb5
-#define DINAC4 0xb8
-#define DINAC5 0xb9
-#define DINBC0 0xc0
-#define DINBC2 0xc2
-#define DINBC4 0xc4
-#define DINBC6 0xc6
-#else
-#error No CONFIG_MPU_SENSORS_xxxx has been defined.
-#endif
-
-
-#endif // DMPKEY_H__
diff --git a/mlsdk/mllite/dmpmap.h b/mlsdk/mllite/dmpmap.h
deleted file mode 100644
index cb53c7c..0000000
--- a/mlsdk/mllite/dmpmap.h
+++ /dev/null
@@ -1,276 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 DMPMAP_H
-#define DMPMAP_H
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-#define DMP_PTAT    0
-#define DMP_XGYR    2
-#define DMP_YGYR    4
-#define DMP_ZGYR    6
-#define DMP_XACC    8
-#define DMP_YACC    10
-#define DMP_ZACC    12
-#define DMP_ADC1    14
-#define DMP_ADC2    16
-#define DMP_ADC3    18
-#define DMP_BIASUNC    20
-#define DMP_FIFORT    22
-#define DMP_INVGSFH    24
-#define DMP_INVGSFL    26
-#define DMP_1H    28
-#define DMP_1L    30
-#define DMP_BLPFSTCH    32
-#define DMP_BLPFSTCL    34
-#define DMP_BLPFSXH    36
-#define DMP_BLPFSXL    38
-#define DMP_BLPFSYH    40
-#define DMP_BLPFSYL    42
-#define DMP_BLPFSZH    44
-#define DMP_BLPFSZL    46
-#define DMP_BLPFMTC    48
-#define DMP_SMC    50
-#define DMP_BLPFMXH    52
-#define DMP_BLPFMXL    54
-#define DMP_BLPFMYH    56
-#define DMP_BLPFMYL    58
-#define DMP_BLPFMZH    60
-#define DMP_BLPFMZL    62
-#define DMP_BLPFC    64
-#define DMP_SMCTH    66
-#define DMP_0H2    68
-#define DMP_0L2    70
-#define DMP_BERR2H    72
-#define DMP_BERR2L    74
-#define DMP_BERR2NH    76
-#define DMP_SMCINC    78
-#define DMP_ANGVBXH    80
-#define DMP_ANGVBXL    82
-#define DMP_ANGVBYH    84
-#define DMP_ANGVBYL    86
-#define DMP_ANGVBZH    88
-#define DMP_ANGVBZL    90
-#define DMP_BERR1H    92
-#define DMP_BERR1L    94
-#define DMP_ATCH    96
-#define DMP_BIASUNCSF    98
-#define DMP_ACT2H    100
-#define DMP_ACT2L    102
-#define DMP_GSFH    104
-#define DMP_GSFL    106
-#define DMP_GH    108
-#define DMP_GL    110
-#define DMP_0_5H    112
-#define DMP_0_5L    114
-#define DMP_0_0H    116
-#define DMP_0_0L    118
-#define DMP_1_0H    120
-#define DMP_1_0L    122
-#define DMP_1_5H    124
-#define DMP_1_5L    126
-#define DMP_TMP1AH    128
-#define DMP_TMP1AL    130
-#define DMP_TMP2AH    132
-#define DMP_TMP2AL    134
-#define DMP_TMP3AH    136
-#define DMP_TMP3AL    138
-#define DMP_TMP4AH    140
-#define DMP_TMP4AL    142
-#define DMP_XACCW    144
-#define DMP_TMP5    146
-#define DMP_XACCB    148
-#define DMP_TMP8    150
-#define DMP_YACCB    152
-#define DMP_TMP9    154
-#define DMP_ZACCB    156
-#define DMP_TMP10    158
-#define DMP_DZH    160
-#define DMP_DZL    162
-#define DMP_XGCH    164
-#define DMP_XGCL    166
-#define DMP_YGCH    168
-#define DMP_YGCL    170
-#define DMP_ZGCH    172
-#define DMP_ZGCL    174
-#define DMP_YACCW    176
-#define DMP_TMP7    178
-#define DMP_AFB1H    180
-#define DMP_AFB1L    182
-#define DMP_AFB2H    184
-#define DMP_AFB2L    186
-#define DMP_MAGFBH    188
-#define DMP_MAGFBL    190
-#define DMP_QT1H    192
-#define DMP_QT1L    194
-#define DMP_QT2H    196
-#define DMP_QT2L    198
-#define DMP_QT3H    200
-#define DMP_QT3L    202
-#define DMP_QT4H    204
-#define DMP_QT4L    206
-#define DMP_CTRL1H    208
-#define DMP_CTRL1L    210
-#define DMP_CTRL2H    212
-#define DMP_CTRL2L    214
-#define DMP_CTRL3H    216
-#define DMP_CTRL3L    218
-#define DMP_CTRL4H    220
-#define DMP_CTRL4L    222
-#define DMP_CTRLS1    224
-#define DMP_CTRLSF1    226
-#define DMP_CTRLS2    228
-#define DMP_CTRLSF2    230
-#define DMP_CTRLS3    232
-#define DMP_CTRLSFNLL    234
-#define DMP_CTRLS4    236
-#define DMP_CTRLSFNL2    238
-#define DMP_CTRLSFNL    240
-#define DMP_TMP30    242
-#define DMP_CTRLSFJT    244
-#define DMP_TMP31    246
-#define DMP_TMP11    248
-#define DMP_CTRLSF2_2    250
-#define DMP_TMP12    252
-#define DMP_CTRLSF1_2    254
-#define DMP_PREVPTAT    256
-#define DMP_ACCZB    258
-#define DMP_ACCXB    264
-#define DMP_ACCYB    266
-#define DMP_1HB    272
-#define DMP_1LB    274
-#define DMP_0H    276
-#define DMP_0L    278
-#define DMP_ASR22H    280
-#define DMP_ASR22L    282
-#define DMP_ASR6H    284
-#define DMP_ASR6L    286
-#define DMP_TMP13    288
-#define DMP_TMP14    290
-#define DMP_FINTXH    292
-#define DMP_FINTXL    294
-#define DMP_FINTYH    296
-#define DMP_FINTYL    298
-#define DMP_FINTZH    300
-#define DMP_FINTZL    302
-#define DMP_TMP1BH    304
-#define DMP_TMP1BL    306
-#define DMP_TMP2BH    308
-#define DMP_TMP2BL    310
-#define DMP_TMP3BH    312
-#define DMP_TMP3BL    314
-#define DMP_TMP4BH    316
-#define DMP_TMP4BL    318
-#define DMP_STXG    320
-#define DMP_ZCTXG    322
-#define DMP_STYG    324
-#define DMP_ZCTYG    326
-#define DMP_STZG    328
-#define DMP_ZCTZG    330
-#define DMP_CTRLSFJT2    332
-#define DMP_CTRLSFJTCNT    334
-#define DMP_PVXG    336
-#define DMP_TMP15    338
-#define DMP_PVYG    340
-#define DMP_TMP16    342
-#define DMP_PVZG    344
-#define DMP_TMP17    346
-#define DMP_MNMFLAGH    352
-#define DMP_MNMFLAGL    354
-#define DMP_MNMTMH    356
-#define DMP_MNMTML    358
-#define DMP_MNMTMTHRH    360
-#define DMP_MNMTMTHRL    362
-#define DMP_MNMTHRH    364
-#define DMP_MNMTHRL    366
-#define DMP_ACCQD4H    368
-#define DMP_ACCQD4L    370
-#define DMP_ACCQD5H    372
-#define DMP_ACCQD5L    374
-#define DMP_ACCQD6H    376
-#define DMP_ACCQD6L    378
-#define DMP_ACCQD7H    380
-#define DMP_ACCQD7L    382
-#define DMP_ACCQD0H    384
-#define DMP_ACCQD0L    386
-#define DMP_ACCQD1H    388
-#define DMP_ACCQD1L    390
-#define DMP_ACCQD2H    392
-#define DMP_ACCQD2L    394
-#define DMP_ACCQD3H    396
-#define DMP_ACCQD3L    398
-#define DMP_XN2H    400
-#define DMP_XN2L    402
-#define DMP_XN1H    404
-#define DMP_XN1L    406
-#define DMP_YN2H    408
-#define DMP_YN2L    410
-#define DMP_YN1H    412
-#define DMP_YN1L    414
-#define DMP_YH    416
-#define DMP_YL    418
-#define DMP_B0H    420
-#define DMP_B0L    422
-#define DMP_A1H    424
-#define DMP_A1L    426
-#define DMP_A2H    428
-#define DMP_A2L    430
-#define DMP_SEM1    432
-#define DMP_FIFOCNT    434
-#define DMP_SH_TH_X    436
-#define DMP_PACKET    438
-#define DMP_SH_TH_Y    440
-#define DMP_FOOTER    442
-#define DMP_SH_TH_Z    444
-#define DMP_TEMP29    448
-#define DMP_TEMP30    450
-#define DMP_XACCB_PRE    452
-#define DMP_XACCB_PREL    454
-#define DMP_YACCB_PRE    456
-#define DMP_YACCB_PREL    458
-#define DMP_ZACCB_PRE    460
-#define DMP_ZACCB_PREL    462
-#define DMP_TMP22    464
-#define DMP_TAP_TIMER    466
-#define DMP_TAP_THX    468
-#define DMP_TAP_THY    472
-#define DMP_TAP_THZ    476
-#define DMP_TAPW_MIN    478
-#define DMP_TMP25    480
-#define DMP_TMP26    482
-#define DMP_TMP27    484
-#define DMP_TMP28    486
-#define DMP_ORIENT    488
-#define DMP_THRSH    490
-#define DMP_ENDIANH    492
-#define DMP_ENDIANL    494
-#define DMP_BLPFNMTCH    496
-#define DMP_BLPFNMTCL    498
-#define DMP_BLPFNMXH    500
-#define DMP_BLPFNMXL    502
-#define DMP_BLPFNMYH    504
-#define DMP_BLPFNMYL    506
-#define DMP_BLPFNMZH    508
-#define DMP_BLPFNMZL    510
-#ifdef __cplusplus
-}
-#endif
-#endif // DMPMAP_H
diff --git a/mlsdk/mllite/invensense.h b/mlsdk/mllite/invensense.h
deleted file mode 100644
index 586dd25..0000000
--- a/mlsdk/mllite/invensense.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/** Main header file for Invensense's basic library.
- */
-#include "accel.h"
-#include "compass.h"
-#include "dmpDefault.h"
-#include "dmpKey.h"
-#include "dmpmap.h"
-#include "ml.h"
-#include "mlBiasNoMotion.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "mlMathFunc.h"
-#include "mlSetGyroBias.h"
-#include "ml_legacy.h"
-#include "ml_mputest.h"
-#include "ml_stored_data.h"
-#include "mlcontrol.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mldmp.h"
-#include "mlinclude.h"
-#include "mlstates.h"
-#include "mlsupervisor.h"
-#include "pressure.h"
diff --git a/mlsdk/mllite/ml.c b/mlsdk/mllite/ml.c
deleted file mode 100644
index 0489f5b..0000000
--- a/mlsdk/mllite/ml.c
+++ /dev/null
@@ -1,1874 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- *
- * $Id: ml.c 5642 2011-06-14 02:26:20Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup ML
- *  @brief  Motion Library APIs.
- *          The Motion Library processes gyroscopes, accelerometers, and
- *          compasses to provide a physical model of the movement for the
- *          sensors.
- *          The results of this processing may be used to control objects
- *          within a user interface environment, detect gestures, track 3D
- *          movement for gaming applications, and analyze the blur created
- *          due to hand movement while taking a picture.
- *
- *  @{
- *      @file   ml.c
- *      @brief  The Motion Library APIs.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "ml.h"
-#include "mldl.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "mlMathFunc.h"
-#include "mlsupervisor.h"
-#include "mlmath.h"
-#include "mlcontrol.h"
-#include "mldl_cfg.h"
-#include "mpu.h"
-#include "accel.h"
-#include "mlos.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlBiasNoMotion.h"
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-ml"
-
-#define ML_MOT_TYPE_NONE 0
-#define ML_MOT_TYPE_NO_MOTION 1
-#define ML_MOT_TYPE_MOTION_DETECTED 2
-
-#define ML_MOT_STATE_MOVING 0
-#define ML_MOT_STATE_NO_MOTION 1
-#define ML_MOT_STATE_BIAS_IN_PROG 2
-
-#define _mlDebug(x)             //{x}
-
-/* Global Variables */
-const unsigned char mlVer[] = { INV_VERSION };
-
-struct inv_params_obj inv_params_obj = {
-    INV_BIAS_UPDATE_FUNC_DEFAULT,   // bias_mode
-    INV_ORIENTATION_MASK_DEFAULT,   // orientation_mask
-    INV_PROCESSED_DATA_CALLBACK_DEFAULT,    // fifo_processed_func
-    INV_ORIENTATION_CALLBACK_DEFAULT,   // orientation_cb_func
-    INV_MOTION_CALLBACK_DEFAULT,    // motion_cb_func
-    INV_STATE_SERIAL_CLOSED     // starting state
-};
-
-struct inv_obj_t inv_obj;
-void *g_mlsl_handle;
-
-typedef struct {
-    // These describe callbacks happening everythime a DMP interrupt is processed
-    int_fast8_t numInterruptProcesses;
-    // Array of function pointers, function being void function taking void
-    inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES];
-
-} tMLXCallbackInterrupt;        // MLX_callback_t
-
-tMLXCallbackInterrupt mlxCallbackInterrupt;
-
-/* --------------- */
-/* -  Functions. - */
-/* --------------- */
-
-inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient);
-inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient);
-unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
-
-/**
- *  @brief  Open serial connection with the MPU device.
- *          This is the entry point of the MPL and must be
- *          called prior to any other function call.
- *
- *  @param  port     System handle for 'port' MPU device is found on.
- *                   The significance of this parameter varies by
- *                   platform. It is passed as 'port' to MLSLSerialOpen.
- *
- *  @return INV_SUCCESS or error code.
- */
-inv_error_t inv_serial_start(char const *port)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    if (inv_get_state() >= INV_STATE_SERIAL_OPENED)
-        return INV_SUCCESS;
-
-    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    result = inv_serial_open(port, &g_mlsl_handle);
-    if (INV_SUCCESS != result) {
-        (void)inv_state_transition(INV_STATE_SERIAL_CLOSED);
-    }
-
-    return result;
-}
-
-/**
- *  @brief  Close the serial communication.
- *          This function needs to be called explicitly to shut down
- *          the communication with the device.  Calling inv_dmp_close()
- *          won't affect the established serial communication.
- *  @return INV_SUCCESS; non-zero error code otherwise.
- */
-inv_error_t inv_serial_stop(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() == INV_STATE_SERIAL_CLOSED)
-        return INV_SUCCESS;
-
-    result = inv_state_transition(INV_STATE_SERIAL_CLOSED);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result);
-    }
-    result = inv_serial_close(g_mlsl_handle);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result);
-    }
-    return result;
-}
-
-/**
- *  @brief  Get the serial file handle to the device.
- *  @return The serial file handle.
- */
-void *inv_get_serial_handle(void)
-{
-    INVENSENSE_FUNC_START;
-    return g_mlsl_handle;
-}
-
-/**
- *  @brief  apply the choosen orientation and full scale range
- *          for gyroscopes, accelerometer, and compass.
- *  @return INV_SUCCESS if successful, a non-zero code otherwise.
- */
-inv_error_t inv_apply_calibration(void)
-{
-    INVENSENSE_FUNC_START;
-    signed char gyroCal[9] = { 0 };
-    signed char accelCal[9] = { 0 };
-    signed char magCal[9] = { 0 };
-    float gyroScale = 2000.f;
-    float accelScale = 0.f;
-    float magScale = 0.f;
-
-    inv_error_t result;
-    int ii;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    for (ii = 0; ii < 9; ii++) {
-        gyroCal[ii] = mldl_cfg->pdata->orientation[ii];
-        accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii];
-        magCal[ii] = mldl_cfg->pdata->compass.orientation[ii];
-    }
-
-    switch (mldl_cfg->full_scale) {
-    case MPU_FS_250DPS:
-        gyroScale = 250.f;
-        break;
-    case MPU_FS_500DPS:
-        gyroScale = 500.f;
-        break;
-    case MPU_FS_1000DPS:
-        gyroScale = 1000.f;
-        break;
-    case MPU_FS_2000DPS:
-        gyroScale = 2000.f;
-        break;
-    default:
-        MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n",
-                 mldl_cfg->full_scale);
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale);
-    inv_obj.accel_sens = (long)(accelScale * 65536L);
-    /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-    inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim;
-#else
-    inv_obj.accel_sens /= 2;
-#endif
-
-    RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale);
-    inv_obj.compass_sens = (long)(magScale * 32768);
-
-    if (inv_get_state() == INV_STATE_DMP_OPENED) {
-        result = inv_set_gyro_calibration(gyroScale, gyroCal);
-        if (INV_SUCCESS != result) {
-            MPL_LOGE("Unable to set Gyro Calibration\n");
-            return result;
-        }
-        result = inv_set_accel_calibration(accelScale, accelCal);
-        if (INV_SUCCESS != result) {
-            MPL_LOGE("Unable to set Accel Calibration\n");
-            return result;
-        }
-        result = inv_set_compass_calibration(magScale, magCal);
-        if (INV_SUCCESS != result) {
-            MPL_LOGE("Unable to set Mag Calibration\n");
-            return result;
-        }
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Setup the DMP to handle the accelerometer endianess.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_apply_endian_accel(void)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[4] = { 0 };
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    int endian = mldl_cfg->accel->endian;
-
-    if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) {
-        endian = EXT_SLAVE_BIG_ENDIAN;
-    }
-    switch (endian) {
-    case EXT_SLAVE_FS8_BIG_ENDIAN:
-    case EXT_SLAVE_FS16_BIG_ENDIAN:
-    case EXT_SLAVE_LITTLE_ENDIAN:
-        regs[0] = 0;
-        regs[1] = 64;
-        regs[2] = 0;
-        regs[3] = 0;
-        break;
-    case EXT_SLAVE_BIG_ENDIAN:
-    default:
-        regs[0] = 0;
-        regs[1] = 0;
-        regs[2] = 64;
-        regs[3] = 0;
-    }
-
-    return inv_set_mpu_memory(KEY_D_1_236, 4, regs);
-}
-
-/**
- * @internal
- * @brief   Initialize MLX data.  This should be called to setup the mlx
- *          output buffers before any motion processing is done.
- */
-void inv_init_ml(void)
-{
-    INVENSENSE_FUNC_START;
-    int ii;
-    long tmp[COMPASS_NUM_AXES];
-    // Set all values to zero by default
-    memset(&inv_obj, 0, sizeof(struct inv_obj_t));
-    memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt));
-
-    inv_obj.compass_correction[0] = 1073741824L;
-    inv_obj.compass_correction_relative[0] = 1073741824L;
-    inv_obj.compass_disturb_correction[0] = 1073741824L;
-    inv_obj.compass_correction_offset[0] = 1073741824L;
-    inv_obj.relative_quat[0] = 1073741824L;
-
-    //Not used with the ST accelerometer
-    inv_obj.no_motion_threshold = 20;   // noMotionThreshold
-    //Not used with the ST accelerometer
-    inv_obj.motion_duration = 1536; // motionDuration
-
-    inv_obj.motion_state = INV_MOTION;  // Motion state
-
-    inv_obj.bias_update_time = 8000;
-    inv_obj.bias_calc_time = 2000;
-
-    inv_obj.internal_motion_state = ML_MOT_STATE_MOVING;
-
-    inv_obj.start_time = inv_get_tick_count();
-
-    inv_obj.compass_cal[0] = 322122560L;
-    inv_obj.compass_cal[4] = 322122560L;
-    inv_obj.compass_cal[8] = 322122560L;
-    inv_obj.compass_sens = 322122560L;  // Should only change when the sensor full-scale range (FSR) is changed.
-
-    for (ii = 0; ii < COMPASS_NUM_AXES; ii++) {
-        inv_obj.compass_scale[ii] = 65536L;
-        inv_obj.compass_test_scale[ii] = 65536L;
-        inv_obj.compass_bias_error[ii] = P_INIT;
-        inv_obj.init_compass_bias[ii] = 0;
-        inv_obj.compass_asa[ii] = (1L << 30);
-    }
-    if (inv_compass_read_scale(tmp) == INV_SUCCESS) {
-        for (ii = 0; ii < COMPASS_NUM_AXES; ii++)
-            inv_obj.compass_asa[ii] = tmp[ii];
-    }
-    inv_obj.got_no_motion_bias = 0;
-    inv_obj.got_compass_bias = 0;
-    inv_obj.compass_state = SF_UNCALIBRATED;
-    inv_obj.acc_state = SF_STARTUP_SETTLE;
-
-    inv_obj.got_init_compass_bias = 0;
-    inv_obj.resetting_compass = 0;
-
-    inv_obj.external_slave_callback = NULL;
-    inv_obj.compass_accuracy = 0;
-
-    inv_obj.factory_temp_comp = 0;
-    inv_obj.got_coarse_heading = 0;
-
-    inv_obj.compass_bias_ptr[0] = P_INIT;
-    inv_obj.compass_bias_ptr[4] = P_INIT;
-    inv_obj.compass_bias_ptr[8] = P_INIT;
-
-    inv_obj.gyro_bias_err = 1310720;
-
-    inv_obj.accel_lpf_gain = 1073744L;
-    inv_obj.no_motion_accel_threshold = 7000000L;
-}
-
-/**
- * @internal
- * @brief   This registers a function to be called for each time the DMP
- *          generates an an interrupt.
- *          It will be called after inv_register_fifo_rate_process() which gets called
- *          every time the FIFO data is processed.
- *          The FIFO does not have to be on for this callback.
- * @param func Function to be called when a DMP interrupt occurs.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func)
-{
-    INVENSENSE_FUNC_START;
-    // Make sure we have not filled up our number of allowable callbacks
-    if (mlxCallbackInterrupt.numInterruptProcesses <=
-        MAX_INTERRUPT_PROCESSES - 1) {
-        int kk;
-        // Make sure we haven't registered this function already
-        for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
-            if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
-                return INV_ERROR_INVALID_PARAMETER;
-            }
-        }
-        // Add new callback
-        mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt.
-                                                numInterruptProcesses] = func;
-        mlxCallbackInterrupt.numInterruptProcesses++;
-        return INV_SUCCESS;
-    }
-    return INV_ERROR_MEMORY_EXAUSTED;
-}
-
-/**
- * @internal
- * @brief This unregisters a function to be called for each DMP interrupt.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func)
-{
-    INVENSENSE_FUNC_START;
-    int kk, jj;
-    // Make sure we haven't registered this function already
-    for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
-        if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
-            // FIXME, we may need a thread block here
-            for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses;
-                 ++jj) {
-                mlxCallbackInterrupt.processInterruptCb[jj - 1] =
-                    mlxCallbackInterrupt.processInterruptCb[jj];
-            }
-            mlxCallbackInterrupt.numInterruptProcesses--;
-            return INV_SUCCESS;
-        }
-    }
-    return INV_ERROR_INVALID_PARAMETER;
-}
-
-/**
- *  @internal
- *  @brief  Run the recorded interrupt process callbacks in the event
- *          of an interrupt.
- */
-void inv_run_dmp_interupt_cb(void)
-{
-    int kk;
-    for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
-        if (mlxCallbackInterrupt.processInterruptCb[kk])
-            mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj);
-    }
-}
-
-/** @internal
-* Resets the Motion/No Motion state which should be called at startup and resume.
-*/
-inv_error_t inv_reset_motion(void)
-{
-    unsigned char regs[8];
-    inv_error_t result;
-
-    inv_obj.motion_state = INV_MOTION;
-    inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
-    inv_obj.no_motion_accel_time = inv_get_tick_count();
-#if defined CONFIG_MPU_SENSORS_MPU3050
-    regs[0] = DINAD8 + 2;
-    regs[1] = DINA0C;
-    regs[2] = DINAD8 + 1;
-    result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-#else
-#endif
-    regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
-    regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
-    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    memset(regs, 0, 8);
-    result = inv_set_mpu_memory(KEY_D_1_96, 8, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    result =
-        inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs));
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    inv_set_motion_state(INV_MOTION);
-    return result;
-}
-
-/**
- * @internal
- * @brief   inv_start_bias_calc starts the bias calculation on the MPU.
- */
-void inv_start_bias_calc(void)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_obj.suspend = 1;
-}
-
-/**
- * @internal
- * @brief   inv_stop_bias_calc stops the bias calculation on the MPU.
- */
-void inv_stop_bias_calc(void)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_obj.suspend = 0;
-}
-
-/**
- *  @brief  inv_update_data fetches data from the fifo and updates the
- *          motion algorithms.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start() must have been called.
- *
- *  @note   Motion algorithm data is constant between calls to inv_update_data
- *
- * @return
- * - INV_SUCCESS
- * - Non-zero error code
- */
-inv_error_t inv_update_data(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-    int_fast8_t got, ftry;
-    uint_fast8_t mpu_interrupt;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (inv_get_state() != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    // Set the maximum number of FIFO packets we want to process
-    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
-        ftry = 100;             // Large enough to process all packets
-    } else {
-        ftry = 1;
-    }
-
-    // Go and process at most ftry number of packets, probably less than ftry
-    result = inv_read_and_process_fifo(ftry, &got);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    // Process all interrupts
-    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1);
-    if (mpu_interrupt) {
-        inv_clear_interrupt_trigger(INTSRC_AUX1);
-    }
-    // Check if interrupt was from MPU
-    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU);
-    if (mpu_interrupt) {
-        inv_clear_interrupt_trigger(INTSRC_MPU);
-    }
-    // Take care of the callbacks that want to be notified when there was an MPU interrupt
-    if (mpu_interrupt) {
-        inv_run_dmp_interupt_cb();
-    }
-
-    result = inv_get_fifo_status();
-    return result;
-}
-
-/**
- *  @brief  inv_check_flag returns the value of a flag.
- *          inv_check_flag can be used to check a number of flags,
- *          allowing users to poll flags rather than register callback
- *          functions. If a flag is set to True when inv_check_flag is called,
- *          the flag is automatically reset.
- *          The flags are:
- *          - INV_RAW_DATA_READY
- *          Indicates that new raw data is available.
- *          - INV_PROCESSED_DATA_READY
- *          Indicates that new processed data is available.
- *          - INV_GOT_GESTURE
- *          Indicates that a gesture has been detected by the gesture engine.
- *          - INV_MOTION_STATE_CHANGE
- *          Indicates that a change has been made from motion to no motion,
- *          or vice versa.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start() must have been called.
- *
- *  @param flag The flag to check.
- *
- * @return TRUE or FALSE state of the flag
- */
-int inv_check_flag(int flag)
-{
-    INVENSENSE_FUNC_START;
-    int flagReturn = inv_obj.flags[flag];
-
-    inv_obj.flags[flag] = 0;
-    return flagReturn;
-}
-
-/**
- *  @brief  Enable generation of the DMP interrupt when Motion or no-motion
- *          is detected
- *  @param on
- *          Boolean to turn the interrupt on or off.
- *  @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_set_motion_interrupt(unsigned char on)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char regs[2] = { 0 };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (on) {
-        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        inv_obj.interrupt_sources |= INV_INT_MOTION;
-    } else {
-        inv_obj.interrupt_sources &= ~INV_INT_MOTION;
-        if (!inv_obj.interrupt_sources) {
-            result = inv_get_dl_cfg_int(0);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-    }
-
-    if (on) {
-        regs[0] = DINAFE;
-    } else {
-        regs[0] = DINAD8;
-    }
-    result = inv_set_mpu_memory(KEY_CFG_7, 1, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- * Enable generation of the DMP interrupt when a FIFO packet is ready
- *
- * @param on Boolean to turn the interrupt on or off
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_set_fifo_interrupt(unsigned char on)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char regs[2] = { 0 };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (on) {
-        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        inv_obj.interrupt_sources |= INV_INT_FIFO;
-    } else {
-        inv_obj.interrupt_sources &= ~INV_INT_FIFO;
-        if (!inv_obj.interrupt_sources) {
-            result = inv_get_dl_cfg_int(0);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-    }
-
-    if (on) {
-        regs[0] = DINAFE;
-    } else {
-        regs[0] = DINAD8;
-    }
-    result = inv_set_mpu_memory(KEY_CFG_6, 1, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- * @brief   Get the current set of DMP interrupt sources.
- *          These interrupts are generated by the DMP and can be
- *          routed to the MPU interrupt line via internal
- *          settings.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- * @return  Currently enabled interrupt sources.  The possible interrupts are:
- *          - INV_INT_FIFO,
- *          - INV_INT_MOTION,
- *          - INV_INT_TAP
- */
-int inv_get_interrupts(void)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return 0;               // error
-
-    return inv_obj.interrupt_sources;
-}
-
-/**
- *  @brief  Sets up the Accelerometer calibration and scale factor.
- *
- *          Please refer to the provided "9-Axis Sensor Fusion Application
- *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
- *          offers a good coverage on the mounting matrices and explains how
- *          to use them.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *  @pre    inv_dmp_start() must <b>NOT</b> have been called.
- *
- *  @see    inv_set_gyro_calibration().
- *  @see    inv_set_compass_calibration().
- *
- *  @param[in]  range
- *                  The range of the accelerometers in g's. An accelerometer
- *                  that has a range of +2g's to -2g's should pass in 2.
- *  @param[in]  orientation
- *                  A 9 element matrix that represents how the accelerometers
- *                  are oriented with respect to the device they are mounted
- *                  in and the reference axis system.
- *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
- *                  This example corresponds to a 3 x 3 identity matrix.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_accel_calibration(float range, signed char *orientation)
-{
-    INVENSENSE_FUNC_START;
-    float cal[9];
-    float scale = range / 32768.f;
-    int kk;
-    unsigned long sf;
-    inv_error_t result;
-    unsigned char regs[4] = { 0, 0, 0, 0 };
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    /* Apply zero g-offset values */
-    if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) {
-        regs[0] = 0x80;
-        regs[1] = 0x0;
-        regs[2] = 0x80;
-        regs[3] = 0x0;
-    }
-
-    if (inv_dmpkey_supported(KEY_D_1_152)) {
-        result = inv_set_mpu_memory(KEY_D_1_152, 4, &regs[0]);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    if (scale == 0) {
-        inv_obj.accel_sens = 0;
-    }
-
-    if (mldl_cfg->accel->id) {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-        unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C };
-#else
-        unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
-#endif
-        struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-        unsigned char regs[3];
-        unsigned short orient;
-
-        for (kk = 0; kk < 9; ++kk) {
-            cal[kk] = scale * orientation[kk];
-            inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
-        }
-
-        orient = inv_orientation_matrix_to_scalar(orientation);
-        regs[0] = tmp[orient & 3];
-        regs[1] = tmp[(orient >> 3) & 3];
-        regs[2] = tmp[(orient >> 6) & 3];
-        result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        regs[0] = DINA26;
-        regs[1] = DINA46;
-#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
-        regs[2] = DINA66;
-#else
-        if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision)
-                == MPU_PRODUCT_KEY_B1_E1_5)
-            regs[2] = DINA76;
-        else
-            regs[2] = DINA66;
-#endif
-        if (orient & 4)
-            regs[0] |= 1;
-        if (orient & 0x20)
-            regs[1] |= 1;
-        if (orient & 0x100)
-            regs[2] |= 1;
-
-        result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) {
-            result = inv_freescale_sensor_fusion_16bit(orient);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) {
-            result = inv_freescale_sensor_fusion_8bit(orient);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-    }
-
-    if (inv_obj.accel_sens != 0) {
-        sf = (1073741824L / inv_obj.accel_sens);
-    } else {
-        sf = 0;
-    }
-    regs[0] = (unsigned char)((sf >> 8) & 0xff);
-    regs[1] = (unsigned char)(sf & 0xff);
-    result = inv_set_mpu_memory(KEY_D_0_108, 2, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  Sets up the Gyro calibration and scale factor.
- *
- *          Please refer to the provided "9-Axis Sensor Fusion Application
- *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
- *          offers a good coverage on the mounting matrices and explains
- *          how to use them.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
- *
- *  @see    inv_set_accel_calibration().
- *  @see    inv_set_compass_calibration().
- *
- *  @param[in]  range
- *                  The range of the gyros in degrees per second. A gyro
- *                  that has a range of +2000 dps to -2000 dps should pass in
- *                  2000.
- *  @param[in] orientation
- *                  A 9 element matrix that represents how the gyro are oriented
- *                  with respect to the device they are mounted in. A typical
- *                  set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This
- *                  example corresponds to a 3 x 3 identity matrix.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_calibration(float range, signed char *orientation)
-{
-    INVENSENSE_FUNC_START;
-
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    int kk;
-    float scale;
-    inv_error_t result;
-
-    unsigned char regs[12] = { 0 };
-    unsigned char maxVal = 0;
-    unsigned char tmpPtr = 0;
-    unsigned char tmpSign = 0;
-    unsigned char i = 0;
-    int sf = 0;
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (mldl_cfg->gyro_sens_trim != 0) {
-        /* adjust the declared range to consider the gyro_sens_trim
-           of this part when different from the default (first dividend) */
-        range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim;
-    }
-
-    scale = range / 32768.f;    // inverse of sensitivity for the given full scale range
-    inv_obj.gyro_sens = (long)(range * 32768L);
-
-    for (kk = 0; kk < 9; ++kk) {
-        inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30));
-        inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30));
-    }
-
-    {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-        unsigned char tmpD = DINA4C;
-        unsigned char tmpE = DINACD;
-        unsigned char tmpF = DINA6C;
-#else
-        unsigned char tmpD = DINAC9;
-        unsigned char tmpE = DINA2C;
-        unsigned char tmpF = DINACB;
-#endif
-        regs[3] = DINA36;
-        regs[4] = DINA56;
-        regs[5] = DINA76;
-
-        for (i = 0; i < 3; i++) {
-            maxVal = 0;
-            tmpSign = 0;
-            if (inv_obj.gyro_orient[0 + 3 * i] < 0)
-                tmpSign = 1;
-            if (ABS(inv_obj.gyro_orient[1 + 3 * i]) >
-                ABS(inv_obj.gyro_orient[0 + 3 * i])) {
-                maxVal = 1;
-                if (inv_obj.gyro_orient[1 + 3 * i] < 0)
-                    tmpSign = 1;
-            }
-            if (ABS(inv_obj.gyro_orient[2 + 3 * i]) >
-                ABS(inv_obj.gyro_orient[1 + 3 * i])) {
-                tmpSign = 0;
-                maxVal = 2;
-                if (inv_obj.gyro_orient[2 + 3 * i] < 0)
-                    tmpSign = 1;
-            }
-            if (maxVal == 0) {
-                regs[tmpPtr++] = tmpD;
-                if (tmpSign)
-                    regs[tmpPtr + 2] |= 0x01;
-            } else if (maxVal == 1) {
-                regs[tmpPtr++] = tmpE;
-                if (tmpSign)
-                    regs[tmpPtr + 2] |= 0x01;
-            } else {
-                regs[tmpPtr++] = tmpF;
-                if (tmpSign)
-                    regs[tmpPtr + 2] |= 0x01;
-            }
-        }
-        result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_set_mpu_memory(KEY_FCFG_3, 3, &regs[3]);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384
-        inv_obj.gyro_sf =
-            (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL);
-        result =
-            inv_set_mpu_memory(KEY_D_0_104, 4,
-                               inv_int32_to_big8(inv_obj.gyro_sf, regs));
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        if (inv_obj.gyro_sens != 0) {
-            sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens);
-        } else {
-            sf = 0;
-        }
-
-        result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs));
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Sets up the Compass calibration and scale factor.
- *
- *          Please refer to the provided "9-Axis Sensor Fusion Application
- *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
- *          offers a good coverage on the mounting matrices and explains
- *          how to use them.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
- *
- *  @see    inv_set_gyro_calibration().
- *  @see    inv_set_accel_calibration().
- *
- *  @param[in] range
- *                  The range of the compass.
- *  @param[in] orientation
- *                  A 9 element matrix that represents how the compass is
- *                  oriented with respect to the device they are mounted in.
- *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
- *                  This example corresponds to a 3 x 3 identity matrix.
- *                  The matrix describes how to go from the chip mounting to
- *                  the body of the device.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_compass_calibration(float range, signed char *orientation)
-{
-    INVENSENSE_FUNC_START;
-    float cal[9];
-    float scale = range / 32768.f;
-    int kk;
-    unsigned short compassId = 0;
-
-    compassId = inv_get_compass_id();
-
-    if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883)
-        || (compassId == COMPASS_ID_LSM303M)) {
-        scale /= 32.0f;
-    }
-
-    for (kk = 0; kk < 9; ++kk) {
-        cal[kk] = scale * orientation[kk];
-        inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
-    }
-
-    inv_obj.compass_sens = (long)(scale * 1073741824L);
-
-    if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) {
-        unsigned char reg0[4] = { 0, 0, 0, 0 };
-        unsigned char regp[4] = { 64, 0, 0, 0 };
-        unsigned char regn[4] = { 64 + 128, 0, 0, 0 };
-        unsigned char *reg;
-        int_fast8_t kk;
-        unsigned short keyList[9] =
-            { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02,
-            KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12,
-            KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22
-        };
-
-        for (kk = 0; kk < 9; ++kk) {
-
-            if (orientation[kk] == 1)
-                reg = regp;
-            else if (orientation[kk] == -1)
-                reg = regn;
-            else
-                reg = reg0;
-            inv_set_mpu_memory(keyList[kk], 4, reg);
-        }
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
-* @internal
-* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup.
-*/
-inv_error_t inv_set_dead_zone(void)
-{
-    unsigned char reg;
-    inv_error_t result;
-    extern struct control_params cntrl_params;
-
-    if (cntrl_params.functions & INV_DEAD_ZONE) {
-        reg = 0x08;
-    } else {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-        reg = 0;
-#else
-        if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
-            reg = 0x2;
-        } else {
-            reg = 0;
-        }
-#endif
-    }
-
-    result = inv_set_mpu_memory(KEY_D_0_163, 1, &reg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- *  @brief  inv_set_bias_update is used to register which algorithms will be
- *          used to automatically reset the gyroscope bias.
- *          The engine INV_BIAS_UPDATE must be enabled for these algorithms to
- *          run.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start()
- *          must <b>NOT</b> have been called.
- *
- *  @param  function    A function or bitwise OR of functions that determine
- *                      how the gyroscope bias will be automatically updated.
- *                      Functions include:
- *                      - INV_NONE or 0,
- *                      - INV_BIAS_FROM_NO_MOTION,
- *                      - INV_BIAS_FROM_GRAVITY,
- *                      - INV_BIAS_FROM_TEMPERATURE,
-                    \ifnot UMPL
- *                      - INV_BIAS_FROM_LPF,
- *                      - INV_MAG_BIAS_FROM_MOTION,
- *                      - INV_MAG_BIAS_FROM_GYRO,
- *                      - INV_ALL.
- *                   \endif
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_bias_update(unsigned short function)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[4];
-    long tmp[3] = { 0, 0, 0 };
-    inv_error_t result = INV_SUCCESS;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    /* do not allow progressive no motion bias tracker to run -
-       it's not fully debugged */
-    function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround
-    MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n");
-
-    // You must use EnableFastNoMotion() to get this feature
-    function &= ~INV_BIAS_FROM_FAST_NO_MOTION;
-
-    // You must use DisableFastNoMotion() to turn this feature off
-    if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION)
-        function |= INV_BIAS_FROM_FAST_NO_MOTION;
-
-    /*--- remove magnetic components from bias tracking
-          if there is no compass ---*/
-    if (!inv_compass_present()) {
-        function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION);
-    } else {
-        function &= ~(INV_BIAS_FROM_LPF);
-    }
-
-    // Enable function sets bias from no motion
-    inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION);
-
-    if (function & INV_BIAS_FROM_NO_MOTION) {
-        inv_enable_bias_no_motion();
-    } else {
-        inv_disable_bias_no_motion();
-    }
-
-    if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
-        regs[0] = DINA80 + 2;
-        regs[1] = DINA2D;
-        regs[2] = DINA55;
-        regs[3] = DINA7D;
-    } else {
-        regs[0] = DINA80 + 7;
-        regs[1] = DINA2D;
-        regs[2] = DINA35;
-        regs[3] = DINA3D;
-    }
-    result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_dead_zone();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) &&
-        !inv_compass_present()) {
-        result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    } else {
-        result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    inv_obj.factory_temp_comp = 0;  // FIXME, workaround
-    if ((mldl_cfg->offset_tc[0] != 0) ||
-        (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) {
-        inv_obj.factory_temp_comp = 1;
-    }
-
-    if (inv_obj.factory_temp_comp == 0) {
-        if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) {
-            result = inv_set_gyro_temp_slope(inv_obj.temp_slope);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        } else {
-            result = inv_set_gyro_temp_slope(tmp);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-    } else {
-        inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE;
-        MPL_LOGV("factory temperature compensation coefficients available - "
-                 "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n");
-    }
-
-    /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on
-           compass and accel data, is to have accelerometer data delivered in the
-           FIFO ----*/
-    if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY)
-         && inv_compass_present())
-        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO)
-        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) {
-        inv_send_accel(INV_ALL, INV_32_BIT);
-        inv_send_gyro(INV_ALL, INV_32_BIT);
-    }
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_motion_state is used to determine if the device is in
- *          a 'motion' or 'no motion' state.
- *          inv_get_motion_state returns INV_MOTION of the device is moving,
- *          or INV_NO_MOTION if the device is not moving.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start()
- *          must have been called.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-int inv_get_motion_state(void)
-{
-    INVENSENSE_FUNC_START;
-    return inv_obj.motion_state;
-}
-
-/**
- *  @brief  inv_set_no_motion_thresh is used to set the threshold for
- *          detecting INV_NO_MOTION
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  thresh  A threshold scaled in degrees per second.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_thresh(float thresh)
-{
-    inv_error_t result = INV_SUCCESS;
-    unsigned char regs[4] = { 0 };
-    long tmp;
-    INVENSENSE_FUNC_START;
-
-    tmp = (long)(thresh * thresh * 2.045f);
-    if (tmp < 0) {
-        return INV_ERROR;
-    } else if (tmp > 8180000L) {
-        return INV_ERROR;
-    }
-    inv_obj.no_motion_threshold = tmp;
-
-    regs[0] = (unsigned char)(tmp >> 24);
-    regs[1] = (unsigned char)((tmp >> 16) & 0xff);
-    regs[2] = (unsigned char)((tmp >> 8) & 0xff);
-    regs[3] = (unsigned char)(tmp & 0xff);
-
-    result = inv_set_mpu_memory(KEY_D_1_108, 4, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_reset_motion();
-    return result;
-}
-
-/**
- *  @brief  inv_set_no_motion_threshAccel is used to set the threshold for
- *          detecting INV_NO_MOTION with accelerometers when Gyros have
- *          been turned off
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  thresh  A threshold in g's scaled by 2^32
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_threshAccel(long thresh)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_obj.no_motion_accel_threshold = thresh;
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_no_motion_time is used to set the time required for
- *          detecting INV_NO_MOTION
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  time    A time in seconds.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_time(float time)
-{
-    inv_error_t result = INV_SUCCESS;
-    unsigned char regs[2] = { 0 };
-    long tmp;
-
-    INVENSENSE_FUNC_START;
-
-    tmp = (long)(time * 200);
-    if (tmp < 0) {
-        return INV_ERROR;
-    } else if (tmp > 65535L) {
-        return INV_ERROR;
-    }
-    inv_obj.motion_duration = (unsigned short)tmp;
-
-    regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
-    regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
-    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_reset_motion();
-    return result;
-}
-
-/**
- *  @brief  inv_get_version is used to get the ML version.
- *
- *  @pre    inv_get_version can be called at any time.
- *
- *  @param  version     inv_get_version writes the ML version
- *                      string pointer to version.
- *
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_get_version(unsigned char **version)
-{
-    INVENSENSE_FUNC_START;
-
-    *version = (unsigned char *)mlVer;  //fixme we are wiping const
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief Check for the presence of the gyro sensor.
- *
- * This is not a physical check but a logical check and the value can change
- * dynamically based on calls to inv_set_mpu_sensors().
- *
- * @return  TRUE if the gyro is enabled FALSE otherwise.
- */
-int inv_get_gyro_present(void)
-{
-    return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO |
-                                                     INV_Z_GYRO);
-}
-
-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;
-}
-
-unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
-{
-    unsigned short scalar;
-    /*
-       XYZ  010_001_000 Identity Matrix
-       XZY  001_010_000
-       YXZ  010_000_001
-       YZX  000_010_001
-       ZXY  001_000_010
-       ZYX  000_001_010
-     */
-
-    scalar = inv_row_2_scale(mtx);
-    scalar |= inv_row_2_scale(mtx + 3) << 3;
-    scalar |= inv_row_2_scale(mtx + 6) << 6;
-
-    return scalar;
-}
-
-/* Setups up the Freescale 16-bit accel for Sensor Fusion
-* @param[in] orient A scalar representation of the orientation
-*  that describes how to go from the Chip Orientation
-*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
-*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
-*/
-inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient)
-{
-    unsigned char rr[3];
-    inv_error_t result;
-
-    orient = orient & 0xdb;
-    switch (orient) {
-    default:
-        // Typically 0x88
-        rr[0] = DINACC;
-        rr[1] = DINACF;
-        rr[2] = DINA0E;
-        break;
-    case 0x50:
-        rr[0] = DINACE;
-        rr[1] = DINA0E;
-        rr[2] = DINACD;
-        break;
-    case 0x81:
-        rr[0] = DINACE;
-        rr[1] = DINACB;
-        rr[2] = DINA0E;
-        break;
-    case 0x11:
-        rr[0] = DINACC;
-        rr[1] = DINA0E;
-        rr[2] = DINACB;
-        break;
-    case 0x42:
-        rr[0] = DINA0A;
-        rr[1] = DINACF;
-        rr[2] = DINACB;
-        break;
-    case 0x0a:
-        rr[0] = DINA0A;
-        rr[1] = DINACB;
-        rr[2] = DINACD;
-        break;
-    }
-    result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr);
-    return result;
-}
-
-/* Setups up the Freescale 8-bit accel for Sensor Fusion
-* @param[in] orient A scalar representation of the orientation
-*  that describes how to go from the Chip Orientation
-*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
-*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
-*/
-inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient)
-{
-    unsigned char regs[27];
-    inv_error_t result;
-    uint_fast8_t kk;
-
-    orient = orient & 0xdb;
-    kk = 0;
-
-    regs[kk++] = DINAC3;
-    regs[kk++] = DINA90 + 14;
-    regs[kk++] = DINAA0 + 9;
-    regs[kk++] = DINA3E;
-    regs[kk++] = DINA5E;
-    regs[kk++] = DINA7E;
-
-    regs[kk++] = DINAC2;
-    regs[kk++] = DINAA0 + 9;
-    regs[kk++] = DINA90 + 9;
-    regs[kk++] = DINAF8 + 2;
-
-    switch (orient) {
-    default:
-        // Typically 0x88
-        regs[kk++] = DINACB;
-
-        regs[kk++] = DINA54;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-
-        regs[kk++] = DINACD;
-        break;
-    case 0x50:
-        regs[kk++] = DINACB;
-
-        regs[kk++] = DINACF;
-
-        regs[kk++] = DINA7C;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        break;
-    case 0x81:
-        regs[kk++] = DINA2C;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-
-        regs[kk++] = DINACD;
-
-        regs[kk++] = DINACB;
-        break;
-    case 0x11:
-        regs[kk++] = DINA2C;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINA28;
-        regs[kk++] = DINACB;
-        regs[kk++] = DINACF;
-        break;
-    case 0x42:
-        regs[kk++] = DINACF;
-        regs[kk++] = DINACD;
-
-        regs[kk++] = DINA7C;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        regs[kk++] = DINA78;
-        break;
-    case 0x0a:
-        regs[kk++] = DINACD;
-
-        regs[kk++] = DINA54;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-        regs[kk++] = DINA50;
-
-        regs[kk++] = DINACF;
-        break;
-    }
-
-    regs[kk++] = DINA90 + 7;
-    regs[kk++] = DINAF8 + 3;
-    regs[kk++] = DINAA0 + 9;
-    regs[kk++] = DINA0E;
-    regs[kk++] = DINA0E;
-    regs[kk++] = DINA0E;
-
-    regs[kk++] = DINAF8 + 1;    // filler
-
-    result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-/**
- * Controlls each sensor and each axis when the motion processing unit is
- * running.  When it is not running, simply records the state for later.
- *
- * NOTE: In this version only full sensors controll is allowed.  Independent
- * axis control will return an error.
- *
- * @param sensors Bit field of each axis desired to be turned on or off
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_set_mpu_sensors(unsigned long sensors)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char state = inv_get_state();
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    inv_error_t result = INV_SUCCESS;
-    unsigned short fifoRate;
-
-    if (state < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) &&
-        ((sensors & INV_THREE_AXIS_ACCEL) != 0)) {
-        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    }
-    if (((sensors & INV_THREE_AXIS_ACCEL) != 0) &&
-        (mldl_cfg->pdata->accel.get_slave_descr == 0)) {
-        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
-    }
-
-    if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) &&
-        ((sensors & INV_THREE_AXIS_COMPASS) != 0)) {
-        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    }
-    if (((sensors & INV_THREE_AXIS_COMPASS) != 0) &&
-        (mldl_cfg->pdata->compass.get_slave_descr == 0)) {
-        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
-    }
-
-    if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) &&
-        ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) {
-        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    }
-    if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) &&
-        (mldl_cfg->pdata->pressure.get_slave_descr == 0)) {
-        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
-    }
-
-    /* DMP was off, and is turning on */
-    if (sensors & INV_DMP_PROCESSOR &&
-        !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
-        struct ext_slave_config config;
-        long odr;
-        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
-        config.len = sizeof(long);
-        config.apply = (state == INV_STATE_DMP_STARTED);
-        config.data = &odr;
-
-        odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000);
-        result = inv_mpu_config_accel(mldl_cfg,
-                                      inv_get_serial_handle(),
-                                      inv_get_serial_handle(), &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
-        odr = MPU_SLAVE_IRQ_TYPE_NONE;
-        result = inv_mpu_config_accel(mldl_cfg,
-                                      inv_get_serial_handle(),
-                                      inv_get_serial_handle(), &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        inv_init_fifo_hardare();
-    }
-
-    if (inv_obj.mode_change_func) {
-        result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    /* Get the fifo rate before changing sensors so if we need to match it */
-    fifoRate = inv_get_fifo_rate();
-    mldl_cfg->requested_sensors = sensors;
-
-    /* inv_dmp_start will turn the sensors on */
-    if (state == INV_STATE_DMP_STARTED) {
-        result = inv_dl_start(sensors);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_reset_motion();
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_dl_stop(~sensors);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    inv_set_fifo_rate(fifoRate);
-
-    if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
-        struct ext_slave_config config;
-        long data;
-
-        config.len = sizeof(long);
-        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
-        config.apply = (state == INV_STATE_DMP_STARTED);
-        config.data = &data;
-        data = MPU_SLAVE_IRQ_TYPE_DATA_READY;
-        result = inv_mpu_config_accel(mldl_cfg,
-                                      inv_get_serial_handle(),
-                                      inv_get_serial_handle(), &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    return result;
-}
-
-void inv_set_mode_change(inv_error_t(*mode_change_func)
-                         (unsigned long, unsigned long))
-{
-    inv_obj.mode_change_func = mode_change_func;
-}
-
-/**
-* Mantis setup
-*/
-#ifdef CONFIG_MPU_SENSORS_MPU6050B1
-inv_error_t inv_set_mpu_6050_config()
-{
-    long temp;
-    inv_error_t result;
-    unsigned char big8[4];
-    unsigned char atc[4];
-    long s[3], s2[3];
-    int kk;
-    struct mldl_cfg *mldlCfg = inv_get_dl_config();
-
-    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                             0x0d, 4, atc);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    temp = atc[3] & 0x3f;
-    if (temp >= 32)
-        temp = temp - 64;
-    temp = (temp << 21) | 0x100000;
-    temp += (1L << 29);
-    temp = -temp;
-    result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    for (kk = 0; kk < 3; ++kk) {
-        s[kk] = atc[kk] & 0x3f;
-        if (s[kk] > 32)
-            s[kk] = s[kk] - 64;
-        s[kk] *= 2516582L;
-    }
-
-    for (kk = 0; kk < 3; ++kk) {
-        s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] +
-            mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] +
-            mldlCfg->pdata->orientation[kk * 3 + 2] * s[2];
-    }
-    result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-#endif
-
-/**
- * @}
- */
diff --git a/mlsdk/mllite/ml.h b/mlsdk/mllite/ml.h
deleted file mode 100644
index 838cd49..0000000
--- a/mlsdk/mllite/ml.h
+++ /dev/null
@@ -1,596 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- *
- * $Id: ml.h 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup ML
- *  @brief  The Motion Library processes gyroscopes and accelerometers to
- *          provide a physical model of the movement of the sensors.
- *          The results of this processing may be used to control objects
- *          within a user interface environment, detect gestures, track 3D
- *          movement for gaming applications, and analyze the blur created
- *          due to hand movement while taking a picture.
- *
- *  @{
- *      @file ml.h
- *      @brief Header file for the Motion Library.
-**/
-
-#ifndef INV_H
-#define INV_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mldmp.h"
-#include "mlsl.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "ml_legacy.h"
-#endif
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-
-/* - Module defines. - */
-
-/*************************************************************************/
-/*  Motion Library Vesion                                                */
-/*************************************************************************/
-
-#define INV_VERSION_MAJOR                 4
-#define INV_VERSION_MINOR                 0
-#define INV_VERSION_SUB_MINOR             0
-
-#define INV_VERSION_MAJOR_STR            "4"
-#define INV_VERSION_MINOR_STR            "0"
-#define INV_VERSION_SUB_MINOR_STR        "0"
-
-#define INV_VERSION_NONE                 ""
-#define INV_VERSION_PROTOTYPE            "ProtoA "
-#define INV_VERSION_ENGINEERING          "EngA "
-#define INV_VERSION_PRE_ALPHA            "Pre-Alpha "
-#define INV_VERSION_ALPHA                "Alpha "
-#define INV_VERSION_BETA                 "Beta "
-#define INV_VERSION_PRODUCTION           "Production "
-
-#ifndef INV_VERSION_TYPE
-#define INV_VERSION_TYPE INV_VERSION_NONE
-#endif
-
-#define INV_VERSION  "InvenSense MPL" " " \
-    "v" INV_VERSION_MAJOR_STR "." INV_VERSION_MINOR_STR "." INV_VERSION_SUB_MINOR_STR " " \
-    INV_VERSION_TYPE \
-    __DATE__ " " __TIME__
-
-/*************************************************************************/
-/*  Motion processing engines                                            */
-/*************************************************************************/
-#define INV_MOTION_DETECT                (0x0004)
-#define INV_BIAS_UPDATE                  (0x0008)
-#define INV_GESTURE                      (0x0020)
-#define INV_CONTROL                      (0x0040)
-#define INV_ORIENTATION                  (0x0100)
-#define INV_PEDOMETER                    (0x0200)
-#define INV_BASIC                        (INV_MOTION_DETECT | INV_BIAS_UPDATE)
-
-/*************************************************************************/
-/*  Data Source - Obsolete                                               */
-/*************************************************************************/
-#define INV_DATA_FIFO                    (0x1)
-#define INV_DATA_POLL                    (0x2)
-
-/*************************************************************************/
-/*  Interrupt Source                                                     */
-/*************************************************************************/
-#define INV_INT_MOTION                   (0x01)
-#define INV_INT_FIFO                     (0x02)
-#define INV_INT_TAP                      (0x04)
-#define INV_INT_ORIENTATION              (0x08)
-#define INV_INT_SHAKE_PITCH              (0x10)
-#define INV_INT_SHAKE_ROLL               (0x20)
-#define INV_INT_SHAKE_YAW                (0x40)
-
-/*************************************************************************/
-/*  Bias update functions                                                */
-/*************************************************************************/
-#define INV_BIAS_FROM_NO_MOTION          0x0001
-#define INV_BIAS_FROM_GRAVITY            0x0002
-#define INV_BIAS_FROM_TEMPERATURE        0x0004
-#define INV_BIAS_FROM_LPF                0x0008
-#define INV_MAG_BIAS_FROM_MOTION         0x0010
-#define INV_MAG_BIAS_FROM_GYRO           0x0020
-#define INV_LEARN_BIAS_FROM_TEMPERATURE  0x0040
-#define INV_AUTO_RESET_MAG_BIAS          0x0080
-#define INV_REJECT_MAG_DISTURBANCE       0x0100
-#define INV_PROGRESSIVE_NO_MOTION        0x0200
-#define INV_BIAS_FROM_FAST_NO_MOTION     0x0400
-
-/*************************************************************************/
-/*  Euler angles and axis names                                          */
-/*************************************************************************/
-#define INV_X_AXIS                       (0x01)
-#define INV_Y_AXIS                       (0x02)
-#define INV_Z_AXIS                       (0x04)
-
-#define INV_PITCH                        (INV_X_AXIS)
-#define INV_ROLL                         (INV_Y_AXIS)
-#define INV_YAW                          (INV_Z_AXIS)
-
-/*************************************************************************/
-/*  Sensor types                                                         */
-/*************************************************************************/
-#define INV_GYROS                        0x0001
-#define INV_ACCELS                       0x0002
-
-/*************************************************************************/
-/*  Motion arrays                                                        */
-/*************************************************************************/
-#define INV_ROTATION_MATRIX              0x0003
-#define INV_QUATERNION                   0x0004
-#define INV_EULER_ANGLES                 0x0005
-#define INV_LINEAR_ACCELERATION          0x0006
-#define INV_LINEAR_ACCELERATION_WORLD    0x0007
-#define INV_GRAVITY                      0x0008
-#define INV_ANGULAR_VELOCITY             0x0009
-
-#define INV_GYRO_CALIBRATION_MATRIX      0x000B
-#define INV_ACCEL_CALIBRATION_MATRIX     0x000C
-#define INV_GYRO_BIAS                    0x000D
-#define INV_ACCEL_BIAS                   0x000E
-#define INV_GYRO_TEMP_SLOPE              0x000F
-
-#define INV_RAW_DATA                     0x0011
-#define INV_DMP_TAP                      0x0012
-#define INV_DMP_TAP2                     0x0021
-
-#define INV_EULER_ANGLES_X               0x0013
-#define INV_EULER_ANGLES_Y               0x0014
-#define INV_EULER_ANGLES_Z               0x0015
-
-#define INV_BIAS_UNCERTAINTY             0x0016
-#define INV_DMP_PACKET_NUMBER            0x0017
-#define INV_FOOTER                       0x0018
-
-#define INV_CONTROL_DATA                 0x0019
-
-#define INV_MAGNETOMETER                 0x001A
-#define INV_PEDLBS                       0x001B
-#define INV_MAG_RAW_DATA                 0x001C
-#define INV_MAG_CALIBRATION_MATRIX       0x001D
-#define INV_MAG_BIAS                     0x001E
-#define INV_HEADING                      0x001F
-
-#define INV_MAG_BIAS_ERROR               0x0020
-
-#define INV_PRESSURE                     0x0021
-#define INV_LOCAL_FIELD                  0x0022
-#define INV_MAG_SCALE                    0x0023
-
-#define INV_RELATIVE_QUATERNION          0x0024
-
-#define SET_QUATERNION                                  0x0001
-#define SET_GYROS                                       0x0002
-#define SET_LINEAR_ACCELERATION                         0x0004
-#define SET_GRAVITY                                     0x0008
-#define SET_ACCELS                                      0x0010
-#define SET_TAP                                         0x0020
-#define SET_PEDLBS                                      0x0040
-#define SET_LINEAR_ACCELERATION_WORLD                   0x0080
-#define SET_CONTROL                                     0x0100
-#define SET_PACKET_NUMBER                               0x4000
-#define SET_FOOTER                                      0x8000
-
-/*************************************************************************/
-/*  Integral reset options                                               */
-/*************************************************************************/
-#define INV_NO_RESET                     0x0000
-#define INV_RESET                        0x0001
-
-/*************************************************************************/
-/*  Motion states                                                        */
-/*************************************************************************/
-#define INV_MOTION                       0x0001
-#define INV_NO_MOTION                    0x0002
-
-/*************************************************************************/
-/* Orientation and Gesture states                                        */
-/*************************************************************************/
-#define INV_STATE_IDLE                   (0)
-#define INV_STATE_RUNNING                (1)
-
-/*************************************************************************/
-/* Gyroscope Temperature Compensation bins                               */
-/*************************************************************************/
-#define BINS                            (25)
-#define PTS_PER_BIN                     (5)
-#define MIN_TEMP                        (-40)
-#define MAX_TEMP                        (+85)
-#define TEMP_PER_BIN                    ((MAX_TEMP - MIN_TEMP) / BINS)
-
-/*************************************************************************/
-/*  Flags                                                                */
-/*************************************************************************/
-#define INV_RAW_DATA_READY               0x0001
-#define INV_PROCESSED_DATA_READY         0x0002
-
-#define INV_GOT_GESTURE                  0x0004
-
-#define INV_MOTION_STATE_CHANGE          0x0006
-#define INV_COMPASS_OFFSET_VALID         0x0007
-
-/*************************************************************************/
-/*  General                                                              */
-/*************************************************************************/
-#define INV_NONE                         (0x0000)
-#define INV_INVALID_FIFO_RATE            (0xFFFF)
-
-/*************************************************************************/
-/*  ML Params Structure Default Values                                   */
-/*************************************************************************/
-#define INV_BIAS_UPDATE_FUNC_DEFAULT               (INV_BIAS_FROM_NO_MOTION | INV_BIAS_FROM_GRAVITY)
-#define INV_ORIENTATION_MASK_DEFAULT               0x3f
-#define INV_PROCESSED_DATA_CALLBACK_DEFAULT           0
-#define INV_ORIENTATION_CALLBACK_DEFAULT              0
-#define INV_MOTION_CALLBACK_DEFAULT                   0
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-/* Priority for various features */
-#define INV_PRIORITY_BUS_ACCEL              100
-#define INV_PRIORITY_EXTERNAL_SLAVE_MAG     110
-#define INV_PRIORITY_FAST_NO_MOTION         120
-#define INV_PRIORITY_BIAS_NO_MOTION         125
-#define INV_PRIORITY_SET_GYRO_BIASES        150
-#define INV_PRIORITY_TEMP_COMP              175
-#define INV_PRIORITY_CONTROL                200
-#define INV_PRIORITY_EIS                    300
-#define INV_PRIORITY_ORIENTATION            400
-#define INV_PRIORITY_PEDOMETER_FULLPOWER    500
-#define INV_PRIORITY_NAVIGATION_PEDOMETER   600
-#define INV_PRIORITY_GESTURE                700
-#define INV_PRIORITY_GLYPH                  800
-
-#define MAX_INTERRUPT_PROCESSES 5
-/* Number of quantized accel samples */
-#define INV_MAX_NUM_ACCEL_SAMPLES (8)
-
-#define PRECISION 10000.f
-#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) {               \
-    range.mantissa = (long)x;                               \
-    range.fraction = (long)((float)(x-(long)x)*PRECISION);  \
-}
-#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) {   \
-    x = (float)(range.mantissa);                \
-    x += ((float)range.fraction/PRECISION);     \
-}
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-struct inv_obj_t {
-        //Calibration parameters
-        /* Raw sensor orientation */
-        long gyro_bias[3];
-        long accel_bias[3];
-        long compass_bias[3];
-
-        /* Cached values after orietnation is applied */
-        long scaled_gyro_bias[3];
-        long scaled_accel_bias[3];
-        long scaled_compass_bias[3];
-
-        long compass_scale[3];
-        long compass_test_bias[3];
-        long compass_test_scale[3];
-        long compass_asa[3];
-        long compass_offsets[3];
-
-        long compass_bias_error[3];
-
-        long got_no_motion_bias;
-        long got_compass_bias;
-        long compass_state;
-        long large_field;
-        long acc_state;
-
-        long factory_temp_comp;
-        long got_coarse_heading;
-        long gyro_temp_bias[3];
-        long prog_no_motion_bias[3];
-
-        long accel_cal[9];
-        // Deprecated, used gyro_orient
-        long gyro_cal[GYRO_NUM_AXES * GYRO_NUM_AXES];
-        long gyro_orient[GYRO_NUM_AXES * GYRO_NUM_AXES];
-        long accel_sens;
-        long compass_cal[9];
-        long gyro_sens;
-        long gyro_sf;
-        long temp_slope[GYRO_NUM_AXES];
-        long compass_sens;
-        long temp_offset[GYRO_NUM_AXES];
-
-        int cal_loaded_flag;
-
-        /* temperature compensation */
-        float x_gyro_coef[3];
-        float y_gyro_coef[3];
-        float z_gyro_coef[3];
-        float x_gyro_temp_data[BINS][PTS_PER_BIN];
-        float y_gyro_temp_data[BINS][PTS_PER_BIN];
-        float z_gyro_temp_data[BINS][PTS_PER_BIN];
-        float temp_data[BINS][PTS_PER_BIN];
-        int temp_ptrs[BINS];
-        long temp_valid_data[BINS];
-
-        long compass_correction[4];
-        long compass_correction_relative[4];
-        long compass_disturb_correction[4];
-        long compass_correction_offset[4];
-        long relative_quat[4];
-        long local_field[3];
-        long new_local_field;
-        long sync_grav_body[3];
-        int gyro_bias_err;
-
-        double compass_bias_ptr[9];
-        double compass_bias_v[3];
-        double compass_prev_m[36];
-        double compass_prev_xty[6];
-
-        int compass_peaks[18];
-        int all_sensors_no_motion;
-
-        long init_compass_bias[3];
-
-        int got_init_compass_bias;
-        int resetting_compass;
-
-        long ang_v_body[GYRO_NUM_AXES];
-        unsigned char compass_raw_data[24]; /* Sensor data plus status etc */
-        long compass_sensor_data[3]; /* Raw sensor data only */
-        long compass_calibrated_data[3];
-        long compass_test_calibrated_data[3];
-        long pressure;
-        inv_error_t (*external_slave_callback)(struct inv_obj_t *);
-        int  compass_accuracy;
-        int compass_overunder;
-
-        unsigned short flags[8];
-        unsigned short suspend;
-
-        long no_motion_threshold;
-        unsigned long motion_duration;
-
-        unsigned short motion_state;
-
-        unsigned short data_mode;
-        unsigned short interrupt_sources;
-
-        unsigned short bias_update_time;
-        short last_motion;
-        unsigned short bias_calc_time;
-
-        unsigned char internal_motion_state;
-        long start_time;
-
-        long accel_lpf_gain;
-        long accel_lpf[3];
-        unsigned long poll_no_motion;
-        long no_motion_accel_threshold;
-        unsigned long no_motion_accel_time;
-        inv_error_t(*mode_change_func) (unsigned long, unsigned long);
-    };
-
-    typedef inv_error_t(*inv_obj_func) (struct inv_obj_t *);
-
-    extern struct inv_obj_t inv_obj;
-
-    /* --------------------- */
-    /* - Params Structure. - */
-    /* --------------------- */
-
-    struct inv_params_obj {
-
-        unsigned short bias_mode;   // A function or bitwise OR of functions that determine how the gyroscope bias will be automatically updated.
-        // Functions include INV_BIAS_FROM_NO_MOTION, INV_BIAS_FROM_GRAVITY, and INV_BIAS_FROM_TEMPERATURE.
-        // The engine INV_BIAS_UPDATE must be enabled for these algorithms to run.
-
-        unsigned short orientation_mask;    // Allows a user to register which orientations will trigger the user defined callback function.
-        // The orientations are INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, and INV_Z_DOWN.
-        // INV_ORIENTATION_ALL is equivalent to INV_X_UP | INV_X_DOWN | INV_Y_UP | INV_Y_DOWN | INV_Z_UP | INV_Z_DOWN.
-
-        void (*fifo_processed_func) (void); // Callback function that triggers when all the processing has been finished by the motion processing engines.
-
-        void (*orientation_cb_func) (unsigned short orient);    // Callback function that will run when a change of orientation is detected.
-        // The new orientation. May be one of INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, or INV_Z_DOWN.
-
-        void (*motion_cb_func) (unsigned short motion_state);   // Callback function that will run when a change of motion state is detected.
-        // The new motion state. May be one of INV_MOTION, or INV_NO_MOTION.
-
-        unsigned char state;
-
-    };
-
-    extern struct inv_params_obj inv_params_obj;
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-
-    inv_error_t inv_serial_start(char const *port);
-    inv_error_t inv_serial_stop(void);
-    inv_error_t inv_set_mpu_sensors(unsigned long sensors);
-    void *inv_get_serial_handle(void);
-
-    /*API for handling the buffer */
-    inv_error_t inv_update_data(void);
-
-    /*API for handling polling */
-    int inv_check_flag(int flag);
-
-    /*API for setting bias update function */
-    inv_error_t inv_set_bias_update(unsigned short biasFunction);
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-    inv_error_t inv_turn_on_bias_from_no_motion(void);
-    inv_error_t inv_turn_off_bias_from_no_motion(void);
-    inv_error_t inv_set_mpu_6050_config(void);
-#endif
-
-    /* Legacy functions for handling augmented data*/
-    inv_error_t inv_get_array(int dataSet, long *data);
-    inv_error_t inv_get_float_array(int dataSet, float *data);
-    inv_error_t inv_set_array(int dataSet, long *data);
-    inv_error_t inv_set_float_array(int dataSet, float *data);
-    /* Individual functions for augmented data, per specific dataset */
-
-
-    inv_error_t inv_get_gyro(long *data);
-    inv_error_t inv_get_accel(long *data);
-    inv_error_t inv_get_temperature(long *data);
-    inv_error_t inv_get_temperature_raw(short *data);
-    inv_error_t inv_get_rot_mat(long *data);
-    inv_error_t inv_get_quaternion(long *data);
-    inv_error_t inv_get_linear_accel(long *data);
-    inv_error_t inv_get_linear_accel_in_world(long *data);
-    inv_error_t inv_get_gravity(long *data);
-    inv_error_t inv_get_angular_velocity(long *data);
-    inv_error_t inv_get_euler_angles(long *data);
-    inv_error_t inv_get_euler_angles_x(long *data);
-    inv_error_t inv_get_euler_angles_y(long *data);
-    inv_error_t inv_get_euler_angles_z(long *data);
-    inv_error_t inv_get_gyro_temp_slope(long *data);
-    inv_error_t inv_get_gyro_bias(long *data);
-    inv_error_t inv_get_accel_bias(long *data);
-    inv_error_t inv_get_mag_bias(long *data);
-    inv_error_t inv_get_gyro_and_accel_sensor(long *data);
-    inv_error_t inv_get_mag_raw_data(long *data);
-    inv_error_t inv_get_magnetometer(long *data);
-    inv_error_t inv_get_pressure(long *data);
-    inv_error_t inv_get_heading(long *data);
-    inv_error_t inv_get_gyro_cal_matrix(long *data);
-    inv_error_t inv_get_accel_cal_matrix(long *data);
-    inv_error_t inv_get_mag_cal_matrix(long *data);
-    inv_error_t inv_get_mag_bias_error(long *data);
-    inv_error_t inv_get_mag_scale(long *data);
-    inv_error_t inv_get_local_field(long *data);
-    inv_error_t inv_get_relative_quaternion(long *data);
-    inv_error_t inv_get_gyro_float(float *data);
-    inv_error_t inv_get_accel_float(float *data);
-    inv_error_t inv_get_temperature_float(float *data);
-    inv_error_t inv_get_rot_mat_float(float *data);
-    inv_error_t inv_get_quaternion_float(float *data);
-    inv_error_t inv_get_linear_accel_float(float *data);
-    inv_error_t inv_get_linear_accel_in_world_float(float *data);
-    inv_error_t inv_get_gravity_float(float *data);
-    inv_error_t inv_get_angular_velocity_float(float *data);
-    inv_error_t inv_get_euler_angles_float(float *data);
-    inv_error_t inv_get_euler_angles_x_float(float *data);
-    inv_error_t inv_get_euler_angles_y_float(float *data);
-    inv_error_t inv_get_euler_angles_z_float(float *data);
-    inv_error_t inv_get_gyro_temp_slope_float(float *data);
-    inv_error_t inv_get_gyro_bias_float(float *data);
-    inv_error_t inv_get_accel_bias_float(float *data);
-    inv_error_t inv_get_mag_bias_float(float *data);
-    inv_error_t inv_get_gyro_and_accel_sensor_float(float *data);
-    inv_error_t inv_get_mag_raw_data_float(float *data);
-    inv_error_t inv_get_magnetometer_float(float *data);
-    inv_error_t inv_get_pressure_float(float *data);
-    inv_error_t inv_get_heading_float(float *data);
-    inv_error_t inv_get_gyro_cal_matrix_float(float *data);
-    inv_error_t inv_get_accel_cal_matrix_float(float *data);
-    inv_error_t inv_get_mag_cal_matrix_float(float *data);
-    inv_error_t inv_get_mag_bias_error_float(float *data);
-    inv_error_t inv_get_mag_scale_float(float *data);
-    inv_error_t inv_get_local_field_float(float *data);
-    inv_error_t inv_get_relative_quaternion_float(float *data);
-    inv_error_t inv_get_compass_accuracy(int *accuracy);
-    inv_error_t inv_set_gyro_bias(long *data);
-    inv_error_t inv_set_accel_bias(long *data);
-    inv_error_t inv_set_mag_bias(long *data);
-    inv_error_t inv_set_gyro_temp_slope(long *data);
-    inv_error_t inv_set_local_field(long *data);
-    inv_error_t inv_set_mag_scale(long *data);
-    inv_error_t inv_set_gyro_temp_slope_float(float *data);
-    inv_error_t inv_set_gyro_bias_float(float *data);
-    inv_error_t inv_set_accel_bias_float(float *data);
-    inv_error_t inv_set_mag_bias_float(float *data);
-    inv_error_t inv_set_local_field_float(float *data);
-    inv_error_t inv_set_mag_scale_float(float *data);
-
-    inv_error_t inv_apply_endian_accel(void);
-    inv_error_t inv_apply_calibration(void);
-    inv_error_t inv_set_gyro_calibration(float range, signed char *orientation);
-    inv_error_t inv_set_accel_calibration(float range,
-                                          signed char *orientation);
-    inv_error_t inv_set_compass_calibration(float range,
-                                            signed char *orientation);
-
-    /*API for detecting change of state */
-     inv_error_t
-        inv_set_motion_callback(void (*func) (unsigned short motion_state));
-    int inv_get_motion_state(void);
-
-    /*API for getting ML version. */
-    inv_error_t inv_get_version(unsigned char **version);
-
-    inv_error_t inv_set_motion_interrupt(unsigned char on);
-    inv_error_t inv_set_fifo_interrupt(unsigned char on);
-
-    int inv_get_interrupts(void);
-
-    /* Simulated DMP */
-    int inv_get_gyro_present(void);
-
-    inv_error_t inv_set_no_motion_time(float time);
-    inv_error_t inv_set_no_motion_thresh(float thresh);
-    inv_error_t inv_set_no_motion_threshAccel(long thresh);
-    inv_error_t inv_reset_motion(void);
-
-    inv_error_t inv_update_bias(void);
-    inv_error_t inv_set_dead_zone(void);
-    void inv_start_bias_calc(void);
-    void inv_stop_bias_calc(void);
-
-    // Private functions shared accross modules
-    void inv_init_ml(void);
-
-    inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func);
-    inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func);
-    void inv_run_dmp_interupt_cb(void);
-    void inv_set_mode_change(inv_error_t(*mode_change_func)
-                              (unsigned long, unsigned long));
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INV_H
-/**
- * @}
- */
diff --git a/mlsdk/mllite/mlBiasNoMotion.c b/mlsdk/mllite/mlBiasNoMotion.c
deleted file mode 100644
index 73321ff..0000000
--- a/mlsdk/mllite/mlBiasNoMotion.c
+++ /dev/null
@@ -1,393 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id:$
- *
- *****************************************************************************/
-
-#include "mlBiasNoMotion.h"
-#include "ml.h"
-#include "mlinclude.h"
-#include "mlos.h"
-#include "mlFIFO.h"
-#include "dmpKey.h"
-#include "accel.h"
-#include "mlMathFunc.h"
-#include "mldl.h"
-#include "mlstates.h"
-#include "mlSetGyroBias.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-BiasNoMot"
-
-
-#define _mlDebug(x)             //{x}
-
-/**
- *  @brief  inv_set_motion_callback is used to register a callback function that
- *          will trigger when a change of motion state is detected.
- *
- *  @pre    inv_dmp_open() 
- *          @ifnot MPL_MF 
- *              or inv_open_low_power_pedometer() 
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start()
- *          must <b>NOT</b> have been called.
- *
- *  @param  func    A user defined callback function accepting a
- *                  motion_state parameter, the new motion state.
- *                  May be one of INV_MOTION or INV_NO_MOTION.
- *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state))
-{
-    INVENSENSE_FUNC_START;
-
-    if ((inv_get_state() != INV_STATE_DMP_OPENED) &&
-        (inv_get_state() != INV_STATE_DMP_STARTED))
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    inv_params_obj.motion_cb_func = func;
-
-    return INV_SUCCESS;
-}
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-/** Turns on the feature to compute gyro bias from No Motion */
-inv_error_t inv_turn_on_bias_from_no_motion()
-{
-    inv_error_t result;
-    unsigned char regs[3] = { 0x0d, DINA35, 0x5d };
-    inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
-    result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
-    return result;
-}
-
-/** Turns off the feature to compute gyro bias from No Motion
-*/
-inv_error_t inv_turn_off_bias_from_no_motion()
-{
-    inv_error_t result;
-    unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 };
-    inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
-    result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
-    return result;
-}
-#endif
-
-inv_error_t inv_update_bias(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char regs[12];
-    short bias[GYRO_NUM_AXES];
-
-    if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION)
-        && inv_get_gyro_present()) {
-
-        regs[0] = DINAA0 + 3;
-        result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        result = inv_get_mpu_memory(KEY_D_1_244, 12, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        inv_convert_bias(regs, bias);
-
-        regs[0] = DINAA0 + 15;
-        result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        result =
-            inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                            MPUREG_TEMP_OUT_H, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        inv_obj.got_no_motion_bias = TRUE;
-    }
-    return INV_SUCCESS;
-}
-
-inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj)
-{
-    long gain;
-    unsigned long timeChange;
-    long rate;
-    inv_error_t result;
-    long accel[3], temp;
-    long long accelMag;
-    unsigned long currentTime;
-    int kk;
-
-    if (!inv_accel_present()) {
-        return INV_SUCCESS;
-    }
-
-    currentTime = inv_get_tick_count();
-
-    // We always run the accel low pass filter at the highest sample rate possible
-    result = inv_get_accel(accel);
-    if (result != INV_ERROR_FEATURE_NOT_ENABLED) {
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        rate = inv_get_fifo_rate() * 5 + 5;
-        if (rate > 200)
-            rate = 200;
-
-        gain = inv_obj->accel_lpf_gain * rate;
-        timeChange = inv_get_fifo_rate();
-
-        accelMag = 0;
-        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-            inv_obj->accel_lpf[kk] =
-                inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]);
-            inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]);
-            temp = accel[0] - inv_obj->accel_lpf[0];
-            accelMag += (long long)temp *temp;
-        }
-
-        if (accelMag > inv_obj->no_motion_accel_threshold) {
-            inv_obj->no_motion_accel_time = currentTime;
-
-            // Check for change of state
-            if (!inv_get_gyro_present())
-                inv_set_motion_state(INV_MOTION);
-
-        } else if ((currentTime - inv_obj->no_motion_accel_time) >
-                   5 * inv_obj->motion_duration) {
-            // We have no motion according to accel
-            // Check fsor change of state
-            if (!inv_get_gyro_present())
-                inv_set_motion_state(INV_NO_MOTION);
-        }
-    }
-    return INV_SUCCESS;
-}
-
-/**
- * @internal
- * @brief   Manually update the motion/no motion status.  This is a 
- *          convienence function for implementations that do not wish to use 
- *          inv_update_data.  
- *          This function can be called periodically to check for the 
- *          'no motion' state and update the internal motion status and bias 
- *          calculations.
- */
-inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[3] = { 0 };
-    unsigned short motionFlag = 0;
-    unsigned long currentTime;
-    inv_error_t result;
-
-    result = MLAccelMotionDetection(inv_obj);
-
-    currentTime = inv_get_tick_count();
-
-    // If it is not time to poll for a no motion event, return
-    if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) &&
-        ((currentTime - inv_obj->poll_no_motion) <= 1000))
-        return INV_SUCCESS;
-
-    inv_obj->poll_no_motion = currentTime;
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
-    if (inv_get_gyro_present()
-        && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
-        static long repeatBiasUpdateTime = 0;
-
-        result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1];
-
-        _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);
-            )
-            if (motionFlag == inv_obj->motion_duration) {
-            if (inv_obj->motion_state == INV_MOTION) {
-                inv_update_bias();
-                repeatBiasUpdateTime = inv_get_tick_count();
-
-                regs[0] = DINAD8 + 1;
-                regs[1] = DINA0C;
-                regs[2] = DINAD8 + 2;
-                result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
-                if (result) {
-                    LOG_RESULT_LOCATION(result);
-                    return result;
-                }
-
-                regs[0] = 0;
-                regs[1] = 5;
-                result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
-                if (result) {
-                    LOG_RESULT_LOCATION(result);
-                    return result;
-                }
-
-                //Trigger no motion callback
-                inv_set_motion_state(INV_NO_MOTION);
-            }
-        }
-        if (motionFlag == 5) {
-            if (inv_obj->motion_state == INV_NO_MOTION) {
-                regs[0] = DINAD8 + 2;
-                regs[1] = DINA0C;
-                regs[2] = DINAD8 + 1;
-                result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
-                if (result) {
-                    LOG_RESULT_LOCATION(result);
-                    return result;
-                }
-
-                regs[0] =
-                    (unsigned char)((inv_obj->motion_duration >> 8) & 0xff);
-                regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff);
-                result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
-                if (result) {
-                    LOG_RESULT_LOCATION(result);
-                    return result;
-                }
-
-                //Trigger no motion callback
-                inv_set_motion_state(INV_MOTION);
-            }
-        }
-        if (inv_obj->motion_state == INV_NO_MOTION) {
-            if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) {
-                inv_update_bias();
-                repeatBiasUpdateTime = inv_get_tick_count();
-            }
-        }
-    }
-#else                           // CONFIG_MPU_SENSORS_MPU3050
-    if (inv_get_gyro_present()
-        && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
-        result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1];
-
-        _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);
-            )
-            if (motionFlag > 0) {
-            unsigned char biasReg[12];
-            long biasTmp2[3], biasTmp[3];
-            int i;
-
-            if (inv_obj->last_motion != motionFlag) {
-                result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg);
-
-                for (i = 0; i < 3; i++) {
-                    biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]);
-                }
-                // Rotate bias vector by the transpose of the orientation matrix
-                for (i = 0; i < 3; ++i) {
-                    biasTmp[i] =
-                        inv_q30_mult(biasTmp2[0],
-                                     inv_obj->gyro_orient[i]) +
-                        inv_q30_mult(biasTmp2[1],
-                                     inv_obj->gyro_orient[i + 3]) +
-                        inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]);
-                }
-                inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L);
-                inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L);
-                inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L);
-            }
-            inv_set_motion_state(INV_NO_MOTION);
-        } else {
-            // We are in a motion state
-            inv_set_motion_state(INV_MOTION);
-        }
-        inv_obj->last_motion = motionFlag;
-
-    }
-#endif                          // CONFIG_MPU_SENSORS_MPU3050
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_enable_bias_no_motion(void)
-{
-    inv_error_t result;
-    inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
-    result =
-        inv_register_fifo_rate_process(MLPollMotionStatus,
-                                       INV_PRIORITY_BIAS_NO_MOTION);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_turn_on_bias_from_no_motion();
-#endif
-    return result;
-}
-
-inv_error_t inv_disable_bias_no_motion(void)
-{
-    inv_error_t result;
-    inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
-    result = inv_unregister_fifo_rate_process(MLPollMotionStatus);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-    defined CONFIG_MPU_SENSORS_MPU6050B1
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_turn_off_bias_from_no_motion();
-#endif
-    return result;
-}
diff --git a/mlsdk/mllite/mlBiasNoMotion.h b/mlsdk/mllite/mlBiasNoMotion.h
deleted file mode 100644
index 030dbf9..0000000
--- a/mlsdk/mllite/mlBiasNoMotion.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef INV_BIAS_NO_MOTION_H__
-#define INV_BIAS_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    inv_error_t inv_enable_bias_no_motion(void);
-    inv_error_t inv_disable_bias_no_motion(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INV_BIAS_NO_MOTION_H__
diff --git a/mlsdk/mllite/mlFIFO.c b/mlsdk/mllite/mlFIFO.c
deleted file mode 100644
index 2c0d2f0..0000000
--- a/mlsdk/mllite/mlFIFO.c
+++ /dev/null
@@ -1,2265 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *******************************************************************************/
-
-/**
- *   @defgroup MLFIFO
- *   @brief Motion Library - FIFO Driver.
- *          The FIFO API Interface.
- *
- *   @{
- *       @file mlFIFO.c
- *       @brief FIFO Interface.
-**/
-
-#include <string.h>
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-#    include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-#    include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-#  include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "dmpKey.h"
-#include "mlMathFunc.h"
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mlstates.h"
-#include "mlsupervisor.h"
-#include "mlos.h"
-#include "mlmath.h"
-#include "accel.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-fifo"
-
-#define FIFO_DEBUG 0
-
-#define REF_QUATERNION             (0)
-#define REF_GYROS                  (REF_QUATERNION + 4)
-#define REF_CONTROL                (REF_GYROS + 3)
-#define REF_RAW                    (REF_CONTROL + 4)
-#define REF_RAW_EXTERNAL           (REF_RAW + 8)
-#define REF_ACCEL                  (REF_RAW_EXTERNAL + 6)
-#define REF_QUANT_ACCEL            (REF_ACCEL + 3)
-#define REF_QUATERNION_6AXIS       (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES)
-#define REF_EIS                    (REF_QUATERNION_6AXIS + 4)
-#define REF_DMP_PACKET             (REF_EIS + 3)
-#define REF_GARBAGE                (REF_DMP_PACKET + 1)
-#define REF_LAST                   (REF_GARBAGE + 1)
-
-long fifo_scale[REF_LAST] = {
-    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion
-    // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2)
-    1501974482L, 1501974482L, 1501974482L,  // Gyro
-    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control
-    (1L << 14),                 // Temperature
-    (1L << 14), (1L << 14), (1L << 14), // Raw Gyro
-    (1L << 14), (1L << 14), (1L << 14), (0),    // Raw Accel, plus padding
-    (1L << 14), (1L << 14), (1L << 14), // Raw External
-    (1L << 14), (1L << 14), (1L << 14), // Raw External
-    (1L << 16), (1L << 16), (1L << 16), // Accel
-    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel
-    (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel
-    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis
-    (1L << 30), (1L << 30), (1L << 30), // EIS
-    (1L << 30),                 // Packet
-    (1L << 30),                 // Garbage
-};
-
-// The scale factors for tap need to match the number in fifo_scale above.
-// fifo_base_offset below may also need to be changed if this is not 8
-#if INV_MAX_NUM_ACCEL_SAMPLES != 8
-#error  INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8
-#endif
-
-#define CONFIG_QUAT                (0)
-#define CONFIG_GYROS               (CONFIG_QUAT + 1)
-#define CONFIG_CONTROL_DATA        (CONFIG_GYROS + 1)
-#define CONFIG_TEMPERATURE         (CONFIG_CONTROL_DATA + 1)
-#define CONFIG_RAW_DATA            (CONFIG_TEMPERATURE + 1)
-#define CONFIG_RAW_EXTERNAL        (CONFIG_RAW_DATA + 1)
-#define CONFIG_ACCEL               (CONFIG_RAW_EXTERNAL + 1)
-#define CONFIG_DMP_QUANT_ACCEL     (CONFIG_ACCEL + 1)
-#define CONFIG_EIS                 (CONFIG_DMP_QUANT_ACCEL + 1)
-#define CONFIG_DMP_PACKET_NUMBER   (CONFIG_EIS + 1)
-#define CONFIG_FOOTER              (CONFIG_DMP_PACKET_NUMBER + 1)
-#define NUMFIFOELEMENTS            (CONFIG_FOOTER + 1)
-
-const int fifo_base_offset[NUMFIFOELEMENTS] = {
-    REF_QUATERNION * 4,
-    REF_GYROS * 4,
-    REF_CONTROL * 4,
-    REF_RAW * 4,
-    REF_RAW * 4 + 4,
-    REF_RAW_EXTERNAL * 4,
-    REF_ACCEL * 4,
-    REF_QUANT_ACCEL * 4,
-    REF_EIS * 4,
-    REF_DMP_PACKET * 4,
-    REF_GARBAGE * 4
-};
-
-struct fifo_obj {
-    void (*fifo_process_cb) (void);
-    long decoded[REF_LAST];
-    long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES];
-    int offsets[REF_LAST * 4];
-    int cache;
-    uint_fast8_t gyro_source;
-    unsigned short fifo_rate;
-    unsigned short sample_step_size_ms;
-    uint_fast16_t fifo_packet_size;
-    uint_fast16_t data_config[NUMFIFOELEMENTS];
-    unsigned char reference_count[REF_LAST];
-    long acc_bias_filt[3];
-    float acc_filter_coef;
-    long gravity_cache[3];
-};
-
-static struct fifo_obj fifo_obj;
-
-#define FIFO_CACHE_TEMPERATURE 1
-#define FIFO_CACHE_GYRO 2
-#define FIFO_CACHE_GRAVITY_BODY 4
-#define FIFO_CACHE_ACC_BIAS 8
-
-struct fifo_rate_obj {
-    // These describe callbacks happening everytime a FIFO block is processed
-    int_fast8_t num_cb;
-    HANDLE mutex;
-    inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES];
-    int priority[MAX_HIGH_RATE_PROCESSES];
-};
-
-struct fifo_rate_obj fifo_rate_obj;
-
-/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old 
- *  accuracy if needed.
- */
-static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements,
-                                           uint_fast16_t accuracy,
-                                           uint_fast8_t configOffset)
-{
-    if (elements) {
-        if (!accuracy)
-            accuracy = fifo_obj.data_config[configOffset];
-        else if (accuracy & INV_16_BIT)
-            if ((fifo_obj.data_config[configOffset] & INV_32_BIT))
-                accuracy = INV_32_BIT;  // 32-bits takes priority
-            else
-                accuracy = INV_16_BIT;
-        else
-            accuracy = INV_32_BIT;
-    } else {
-        accuracy = 0;
-    }
-
-    return accuracy;
-}
-
-/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, 
- * the reference counts are subtracted, otherwise they are incremented for each
- * bit set in element. The value returned are the elements that should be sent
- * out as data through the FIFO.
-*/
-static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements,
-                                            uint_fast16_t increment,
-                                            uint_fast8_t refOffset,
-                                            uint_fast8_t len)
-{
-    uint_fast8_t kk;
-
-    if (increment == 0) {
-        for (kk = 0; kk < len; ++kk) {
-            if ((elements & 1)
-                && (fifo_obj.reference_count[kk + refOffset] > 0)) {
-                fifo_obj.reference_count[kk + refOffset]--;
-            }
-            elements >>= 1;
-        }
-    } else {
-        for (kk = 0; kk < len; ++kk) {
-            if (elements & 1)
-                fifo_obj.reference_count[kk + refOffset]++;
-            elements >>= 1;
-        }
-    }
-    elements = 0;
-    for (kk = 0; kk < len; ++kk) {
-        if (fifo_obj.reference_count[kk + refOffset] > 0)
-            elements |= (1 << kk);
-    }
-    return elements;
-}
-
-/**
- * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send
- *  out the FIFO, 0 when removing from the FIFO.
- */
-static inv_error_t inv_construct3_fifo(unsigned char *regs,
-                                       uint_fast16_t elements,
-                                       uint_fast16_t accuracy,
-                                       uint_fast8_t refOffset,
-                                       unsigned short key,
-                                       uint_fast8_t configOffset)
-{
-    int_fast8_t kk;
-    inv_error_t result;
-
-    elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3);
-    accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset);
-
-    if (accuracy & INV_16_BIT) {
-        regs[0] = DINAF8 + 2;
-    }
-
-    fifo_obj.data_config[configOffset] = elements | accuracy;
-
-    for (kk = 0; kk < 3; ++kk) {
-        if ((elements & 1) == 0)
-            regs[kk + 1] = DINAA0 + 3;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(key, 4, regs);
-
-    return result;
-}
-
-/** 
- * @internal
- * Puts footer on FIFO data.
- */
-static inv_error_t inv_set_footer(void)
-{
-    unsigned char regs = DINA30;
-    uint_fast8_t tmp_count;
-    int_fast8_t i, j;
-    int offset;
-    int result;
-    int *fifo_offsets_ptr = fifo_obj.offsets;
-
-    fifo_obj.fifo_packet_size = 0;
-    for (i = 0; i < NUMFIFOELEMENTS; i++) {
-        tmp_count = 0;
-        offset = fifo_base_offset[i];
-        for (j = 0; j < 8; j++) {
-            if ((fifo_obj.data_config[i] >> j) & 0x0001) {
-#ifndef BIG_ENDIAN
-                // Special Case for Byte Ordering on Accel Data
-                if ((i == CONFIG_RAW_DATA) && (j > 2)) {
-                    tmp_count += 2;
-                    switch (inv_get_dl_config()->accel->endian) {
-                    case EXT_SLAVE_BIG_ENDIAN:
-                        *fifo_offsets_ptr++ = offset + 3;
-                        *fifo_offsets_ptr++ = offset + 2;
-                        break;
-                    case EXT_SLAVE_LITTLE_ENDIAN:
-                        *fifo_offsets_ptr++ = offset + 2;
-                        *fifo_offsets_ptr++ = offset + 3;
-                        break;
-                    case EXT_SLAVE_FS8_BIG_ENDIAN:
-                        if (j == 3) {
-                            // Throw this byte away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ = offset + 3;
-                        } else if (j == 4) {
-                            *fifo_offsets_ptr++ = offset + 3;
-                            *fifo_offsets_ptr++ = offset + 7;
-                        } else {
-                            // Throw these byte away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                        }
-                        break;
-                    case EXT_SLAVE_FS16_BIG_ENDIAN:
-                        if (j == 3) {
-                            // Throw this byte away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ = offset + 3;
-                        } else if (j == 4) {
-                            *fifo_offsets_ptr++ = offset - 2;
-                            *fifo_offsets_ptr++ = offset + 3;
-                        } else {
-                            *fifo_offsets_ptr++ = offset - 2;
-                            *fifo_offsets_ptr++ = offset + 3;
-                        }
-                        break;
-                    default:
-                        return INV_ERROR;    // Bad value on ordering
-                    }
-                } else {
-                    tmp_count += 2;
-                    *fifo_offsets_ptr++ = offset + 3;
-                    *fifo_offsets_ptr++ = offset + 2;
-                    if (fifo_obj.data_config[i] & INV_32_BIT) {
-                        *fifo_offsets_ptr++ = offset + 1;
-                        *fifo_offsets_ptr++ = offset;
-                        tmp_count += 2;
-                    }
-                }
-#else
-                // Big Endian Platform
-                // Special Case for Byte Ordering on Accel Data
-                if ((i == CONFIG_RAW_DATA) && (j > 2)) {
-                    tmp_count += 2;
-                    switch (inv_get_dl_config()->accel->endian) {
-                    case EXT_SLAVE_BIG_ENDIAN:
-                        *fifo_offsets_ptr++ = offset + 2;
-                        *fifo_offsets_ptr++ = offset + 3;
-                        break;
-                    case EXT_SLAVE_LITTLE_ENDIAN:
-                        *fifo_offsets_ptr++ = offset + 3;
-                        *fifo_offsets_ptr++ = offset + 2;
-                        break;
-                    case EXT_SLAVE_FS8_BIG_ENDIAN:
-                        if (j == 3) {
-                            // Throw this byte away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ = offset;
-                        } else if (j == 4) {
-                            *fifo_offsets_ptr++ = offset;
-                            *fifo_offsets_ptr++ = offset + 4;
-                        } else {
-                            // Throw these bytes away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                        }
-                        break;
-                    case EXT_SLAVE_FS16_BIG_ENDIAN:
-                        if (j == 3) {
-                            // Throw this byte away
-                            *fifo_offsets_ptr++ =
-                                fifo_base_offset[CONFIG_FOOTER];
-                            *fifo_offsets_ptr++ = offset;
-                        } else if (j == 4) {
-                            *fifo_offsets_ptr++ = offset - 3;
-                            *fifo_offsets_ptr++ = offset;
-                        } else {
-                            *fifo_offsets_ptr++ = offset - 3;
-                            *fifo_offsets_ptr++ = offset;
-                        }
-                        break;
-                    default:
-                        return INV_ERROR;    // Bad value on ordering
-                    }
-                } else {
-                    tmp_count += 2;
-                    *fifo_offsets_ptr++ = offset;
-                    *fifo_offsets_ptr++ = offset + 1;
-                    if (fifo_obj.data_config[i] & INV_32_BIT) {
-                        *fifo_offsets_ptr++ = offset + 2;
-                        *fifo_offsets_ptr++ = offset + 3;
-                        tmp_count += 2;
-                    }
-                }
-
-#endif
-            }
-            offset += 4;
-        }
-        fifo_obj.fifo_packet_size += tmp_count;
-    }
-    if (fifo_obj.data_config[CONFIG_FOOTER] == 0 &&
-        fifo_obj.fifo_packet_size > 0) {
-        // Add footer
-        result = inv_set_mpu_memory(KEY_CFG_16, 1, &regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT;
-        fifo_obj.fifo_packet_size += 2;
-    } else if (fifo_obj.data_config[CONFIG_FOOTER] &&
-               (fifo_obj.fifo_packet_size == 2)) {
-        // Remove Footer
-        regs = DINAA0 + 3;
-        result = inv_set_mpu_memory(KEY_CFG_16, 1, &regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        fifo_obj.data_config[CONFIG_FOOTER] = 0;
-        fifo_obj.fifo_packet_size = 0;
-    }
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_decode_quantized_accel(void)
-{
-    int kk;
-    int fifoRate = inv_get_fifo_rate();
-
-    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1));
-         kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) {
-        union {
-            unsigned int u10:10;
-            signed int s10:10;
-        } temp;
-
-        union {
-            uint32_t u32;
-            int32_t s32;
-        } value;
-
-        value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk];
-        // unquantize this samples.  
-        // They are stored as x * 2^20 + y * 2^10 + z
-        // Z
-        temp.u10 = value.u32 & 0x3ff;
-        value.s32 -= temp.s10;
-        fifo_obj.decoded_accel[kk][2] = temp.s10 * 64;
-        // Y
-        value.s32 = value.s32 / 1024;
-        temp.u10 = value.u32 & 0x3ff;
-        value.s32 -= temp.s10;
-        fifo_obj.decoded_accel[kk][1] = temp.s10 * 64;
-        // X
-        value.s32 = value.s32 / 1024;
-        temp.u10 = value.u32 & 0x3ff;
-        fifo_obj.decoded_accel[kk][0] = temp.s10 * 64;
-    }
-    return INV_SUCCESS;
-}
-
-static inv_error_t inv_state_change_fifo(unsigned char newState)
-{
-    inv_error_t result = INV_SUCCESS;
-    unsigned char regs[4];
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    /* Don't reset the fifo on a fifo rate change */
-    if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
-        (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) {
-        /* Delay output on restart by 50ms due to warm up time of gyros */
-
-        short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1);
-        inv_init_fifo_hardare();
-        inv_int16_to_big8(delay, regs);
-        result = inv_set_mpu_memory(KEY_D_1_178, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    if (INV_STATE_DMP_STARTED == newState) {
-        if (inv_dmpkey_supported(KEY_D_1_128)) {
-            double tmp;
-            tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1);
-            if (tmp > 0x40000000L)
-                tmp = 0x40000000L;
-            (void)inv_int32_to_big8((long)tmp, regs);
-            result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            result = inv_reset_fifo();
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-    }
-    return result;
-}
-
-/**
- * @internal
- * @brief get the FIFO packet size
- * @return the FIFO packet size
- */
-uint_fast16_t inv_get_fifo_packet_size(void)
-{
-    return fifo_obj.fifo_packet_size;
-}
-
-/**
- *  @brief  Initializes all the internal static variables for 
- *          the FIFO module.
- *  @note   Should be called by the initialization routine such 
- *          as inv_dmp_open().
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise. 
- */
-inv_error_t inv_init_fifo_param(void)
-{
-    inv_error_t result;
-    memset(&fifo_obj, 0, sizeof(struct fifo_obj));
-    fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity
-    inv_set_linear_accel_filter_coef(0.f);
-    fifo_obj.fifo_rate = 20;
-    fifo_obj.sample_step_size_ms = 100;
-    memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
-    result = inv_create_mutex(&fifo_rate_obj.mutex);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_register_state_callback(inv_state_change_fifo);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- *  @brief  Close the FIFO usage.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_close_fifo(void)
-{
-    inv_error_t result;
-    inv_error_t first = INV_SUCCESS;
-    result = inv_unregister_state_callback(inv_state_change_fifo);
-    ERROR_CHECK_FIRST(first, result);
-    result = inv_destroy_mutex(fifo_rate_obj.mutex);
-    ERROR_CHECK_FIRST(first, result);
-    memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
-    return first;
-}
-
-/** 
- * Set the gyro source to output to the fifo
- * 
- * @param source The source.  One of 
- * - INV_GYRO_FROM_RAW
- * - INV_GYRO_FROM_QUATERNION
- * 
- * @return INV_SUCCESS or non-zero error code;
- */
-inv_error_t inv_set_gyro_data_source(uint_fast8_t source)
-{
-    if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    fifo_obj.gyro_source = source;
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief  Reads and processes FIFO data. Also handles callbacks when data is
- *          ready.
- *  @param  numPackets 
- *              Number of FIFO packets to try to read. You should
- *              use a large number here, such as 100, if you want to read all
- *              the full packets in the FIFO, which is typical operation.
- *  @param  processed
- *              The number of FIFO packets processed. This may be incremented
- *              even if high rate processes later fail.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
-                                      int_fast8_t * processed)
-{
-    int_fast8_t packet;
-    inv_error_t result = INV_SUCCESS;
-    uint_fast16_t read;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    int kk;
-
-    if (NULL == processed)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    *processed = 0;
-    if (fifo_obj.fifo_packet_size == 0)
-        return result;          // Nothing to read
-
-    for (packet = 0; packet < numPackets; ++packet) {
-        if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
-            unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE];
-            unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE];
-            read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size,
-                                footer_n_data);
-            if (0 == read ||
-                read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) {
-                result = inv_get_fifo_status();
-                if (INV_SUCCESS != result) {
-                    memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
-                }
-                return result;
-            }
-
-            result = inv_process_fifo_packet(buf);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        } else if (inv_accel_present()) {
-            long data[ACCEL_NUM_AXES];
-            result = inv_get_accel_data(data);
-            if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
-                return INV_SUCCESS;
-            }
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-
-            memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
-            fifo_obj.cache = 0;
-            for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-                fifo_obj.decoded[REF_RAW + 4 + kk] =
-                    inv_q30_mult((data[kk] << 16),
-                                 fifo_scale[REF_RAW + 4 + kk]);
-                fifo_obj.decoded[REF_ACCEL + kk] =
-                    inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]);
-                fifo_obj.decoded[REF_ACCEL + kk] -=
-                    inv_obj.scaled_accel_bias[kk];
-            }
-        }
-        // The buffer was processed correctly, so increment even if
-        // other processes fail later, which will return an error
-        *processed = *processed + 1;
-
-        if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) &&
-            fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) {
-            result = inv_decode_quantized_accel();
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-
-        if (fifo_obj.data_config[CONFIG_QUAT]) {
-            result = inv_accel_compass_supervisor();
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-
-        result = inv_pressure_supervisor();
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        // Callbacks now that we have a buffer of data ready
-        result = inv_run_fifo_rate_processes();
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-    }
-    return result;
-}
-
-/**
- *  @brief  inv_set_fifo_processed_callback is used to set a processed data callback
- *          function.  inv_set_fifo_processed_callback sets a user defined callback
- *          function that triggers when all the decoding has been finished by
- *          the motion processing engines. It is called before other bigger 
- *          processing engines to allow lower latency for the user.
- *
- *  @pre    inv_dmp_open() 
- *          @ifnot MPL_MF 
- *              or inv_open_low_power_pedometer() 
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start() 
- *          must <b>NOT</b> have been called.
- *
- *  @param  func    A user defined callback function.
- *
- *  @return INV_SUCCESS if successful, or non-zero error code otherwise.
- */
-inv_error_t inv_set_fifo_processed_callback(void (*func) (void))
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    fifo_obj.fifo_process_cb = func;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @internal
- * @brief   Process data from the dmp read via the fifo.  Takes a buffer 
- *          filled with bytes read from the DMP FIFO. 
- *          Currently expects only the 16 bytes of quaternion data. 
- *          Calculates the motion parameters from that data and stores the 
- *          results in an internal data structure.
- * @param[in,out]   dmpData     Pointer to the buffer containing the fifo data.
- * @return  INV_SUCCESS or error code.
-**/
-inv_error_t inv_process_fifo_packet(const unsigned char *dmpData)
-{
-    INVENSENSE_FUNC_START;
-    int N, kk;
-    unsigned char *p;
-
-    p = (unsigned char *)(&fifo_obj.decoded);
-    N = fifo_obj.fifo_packet_size;
-    if (N > sizeof(fifo_obj.decoded))
-        return INV_ERROR_ASSERTION_FAILURE;
-
-    memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
-
-    for (kk = 0; kk < N; ++kk) {
-        p[fifo_obj.offsets[kk]] = *dmpData++;
-    }
-
-    // If multiplies are much greater cost than if checks, you could check
-    // to see if fifo_scale is non-zero first, or equal to (1L<<30)
-    for (kk = 0; kk < REF_LAST; ++kk) {
-        fifo_obj.decoded[kk] =
-            inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]);
-    }
-
-    memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS],
-           &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long));
-
-    inv_obj.flags[INV_PROCESSED_DATA_READY] = 1;
-    fifo_obj.cache = 0;
-
-    return INV_SUCCESS;
-}
-
-/** Converts 16-bit temperature data as read from temperature register
-* into Celcius scaled by 2^16.
-*/
-long inv_decode_temperature(short tempReg)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-    // Celcius = 35 + (T + 3048.7)/325.9
-    return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L);
-#endif
-#if defined CONFIG_MPU_SENSORS_MPU6050B1
-    // Celcius = 35 + (T + 927.4)/360.6
-    return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L);
-#endif
-#if defined CONFIG_MPU_SENSORS_MPU3050
-    // Celcius = 35 + (T + 13200)/280
-    return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L);
-#endif
-}
-
-/**  @internal
-* Returns the temperature in hardware units. The scaling may change from part to part.
-*/
-inv_error_t inv_get_temperature_raw(short *data)
-{
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) {
-        inv_error_t result;
-        unsigned char regs[2];
-        if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) {
-            if (FIFO_DEBUG)
-                MPL_LOGI("Fetching the temperature from the registers\n");
-            fifo_obj.cache |= FIFO_CACHE_TEMPERATURE;
-            result = inv_serial_read(inv_get_serial_handle(),
-                                inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2,
-                                regs);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]);
-        }
-    }
-    *data = (short)fifo_obj.decoded[REF_RAW];
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 1-element vector of temperature. It is read from the hardware if it
- *              doesn't exist in the FIFO.
- *  @param[out] data    1-element vector of temperature
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_temperature(long *data)
-{
-    short tr;
-    inv_error_t result;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-    result = inv_get_temperature_raw(&tr);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    data[0] = inv_decode_temperature(tr);
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Get the Decoded Accel Data.
- *  @param  data
- *              a buffer to store the quantized data.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_get_unquantized_accel(long *data)
-{
-    int ii, kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
-        for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
-            data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk];
-        }
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Get the Quantized Accel data algorithm output from the FIFO.
- *  @param  data
- *              a buffer to store the quantized data.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_get_quantized_accel(long *data)
-{
-    int ii;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
-        data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii];
-    }
-
-    return INV_SUCCESS;
-}
-
-/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO
-*  and it is read from the registers if it was not put into the FIFO. The data is
-*  cached till the next FIFO processing block time.
-* @param[out] data Length 3, Gyro data
-*/
-inv_error_t inv_get_gyro_sensor(long *data)
-{
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-    if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) {
-        inv_error_t result;
-        unsigned char regs[6];
-        if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) {
-            fifo_obj.cache |= FIFO_CACHE_GYRO;
-            result =
-                inv_serial_read(inv_get_serial_handle(),
-                                inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6,
-                                regs);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            fifo_obj.decoded[REF_RAW + 1] =
-                (((long)regs[0]) << 24) | (((long)regs[1]) << 16);
-            fifo_obj.decoded[REF_RAW + 2] =
-                (((long)regs[2]) << 24) | (((long)regs[3]) << 16);
-            fifo_obj.decoded[REF_RAW + 3] =
-                (((long)regs[4]) << 24) | (((long)regs[5]) << 16);
-
-            // Temperature starts at location 0, Gyro at location 1.
-            fifo_obj.decoded[REF_RAW + 1] =
-                inv_q30_mult(fifo_obj.decoded[REF_RAW + 1],
-                             fifo_scale[REF_RAW + 1]);
-            fifo_obj.decoded[REF_RAW + 2] =
-                inv_q30_mult(fifo_obj.decoded[REF_RAW + 2],
-                             fifo_scale[REF_RAW + 2]);
-            fifo_obj.decoded[REF_RAW + 3] =
-                inv_q30_mult(fifo_obj.decoded[REF_RAW + 3],
-                             fifo_scale[REF_RAW + 3]);
-        }
-        data[0] = fifo_obj.decoded[REF_RAW + 1];
-        data[1] = fifo_obj.decoded[REF_RAW + 2];
-        data[2] = fifo_obj.decoded[REF_RAW + 3];
-    } else {
-        long data2[6];
-        inv_get_gyro_and_accel_sensor(data2);
-        data[0] = data2[0];
-        data[1] = data2[1];
-        data[2] = data2[2];
-    }
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 6-element vector of gyro and accel data
- *  @param[out] data    6-element vector of gyro and accel data
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_gyro_and_accel_sensor(long *data)
-{
-    int ii;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_RAW_DATA])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) {
-        data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii];
-    }
-
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 3-element vector of external sensor
- *  @param[out] data    3-element vector of external sensor
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_external_sensor_data(long *data, int size)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-    int ii;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (ii = 0; ii < size && ii < 6; ii++) {
-        data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii];
-    }
-
-    return INV_SUCCESS;
-#else
-    memset(data, 0, COMPASS_NUM_AXES * sizeof(long));
-    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-#endif
-}
-
-/** 
- *  Sends accelerometer data to the FIFO.
- *
- *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis
- *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- *            for a subset.
- *
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *            bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 };
-    inv_error_t result;
-    int kk;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL,
-                                 KEY_CFG_12, CONFIG_ACCEL);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
-        fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens;
-    }
-
-    return inv_set_footer();
-}
-
-/** 
- * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits.
- *
- *  @param[in] elements Which of the 4 elements to send. Use INV_ALL for all
- *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd
- *             together for a subset.
- *
- *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *             bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    int_fast8_t kk;
-    inv_error_t result;
-    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4);
-    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA);
-
-    if (accuracy & INV_16_BIT) {
-        regs[0] = DINAF8 + 2;
-    }
-
-    fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy;
-
-    for (kk = 0; kk < 4; ++kk) {
-        if ((elements & 1) == 0)
-            regs[kk + 1] = DINAA0 + 3;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_1, 5, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-/** 
- * Adds a rolling counter to the fifo packet.  When used with the footer
- * the data comes out the first time:
- * 
- * @code
- * <data0><data1>...<dataN><PacketNum0><PacketNum1>
- * @endcode
- * for every other packet it is
- *
- * @code
- * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1>
- * @endcode
- *
- * This allows for scanning of the fifo for packets
- * 
- * @return INV_SUCCESS or error code
- */
-inv_error_t inv_send_packet_number(uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char regs;
-    uint_fast16_t elements;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1);
-    if (elements & 1) {
-        regs = DINA28;
-        fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] =
-            INV_ELEMENT_1 | INV_16_BIT;
-    } else {
-        regs = DINAF8 + 3;
-        fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0;
-    }
-    result = inv_set_mpu_memory(KEY_CFG_23, 1, &regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-/**
- *  @brief  Send the computed gravity vectors into the FIFO.
- *          The gravity vectors can be retrieved from the FIFO via 
- *          inv_get_gravity(), to have the gravitation vector expressed
- *          in coordinates relative to the body.
- *
- *  Gravity is a derived vector derived from the quaternion.
- *  @param  elements
- *              the gravitation vectors components bitmask.
- *              To send all compoents use INV_ALL.
- *  @param  accuracy
- *              The number of bits the gravitation vector is expressed 
- *              into.
- *              Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *              bit data. 
- *              Set to zero to remove it from the FIFO.
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    result = inv_send_quaternion(accuracy);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-/** Sends gyro data to the FIFO. Gyro data is a 3 length vector
- *  of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
- *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
- *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- *            for a subset.
- *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *             bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 };
-    inv_error_t result;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) {
-        regs[0] = DINA90 + 5;
-        result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        regs[0] = DINAF8 + 1;
-        regs[1] = DINA20;
-        regs[2] = DINA28;
-        regs[3] = DINA30;
-    } else {
-        regs[0] = DINA90 + 10;
-        result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        regs[0] = DINAF8 + 1;
-        regs[1] = DINA28;
-        regs[2] = DINA30;
-        regs[3] = DINA38;
-    }
-    result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS,
-                                 KEY_CFG_9, CONFIG_GYROS);
-
-    return inv_set_footer();
-}
-
-/** Sends linear accelerometer data to the FIFO. 
- *
- *  Linear accelerometer data is a 3 length vector of 32-bits. It is the 
- *  acceleration in the body frame with gravity removed.
- * 
- * 
- *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
- *            them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- *            for a subset.
- *
- *  NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES
- *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *             bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_linear_accel(uint_fast16_t elements,
-                                  uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char state = inv_get_state();
-
-    if (state < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    result = inv_send_gravity(elements, accuracy);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_send_accel(elements, accuracy);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-/** Sends linear world accelerometer data to the FIFO. Linear world
- *  accelerometer data is a 3 length vector of 32-bits. It is the acceleration
- *  in the world frame with gravity removed. Should be called once after
- *  inv_dmp_open() and before inv_dmp_start().
- *
- *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 
- *             them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- *             for a subset.
- *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *             bit data.
- */
-inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
-                                           uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    result = inv_send_linear_accel(INV_ALL, accuracy);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_send_quaternion(accuracy);
-
-    return inv_set_footer();
-}
-
-/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector
- *   of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- *            bit data.
- */
-inv_error_t inv_send_quaternion(uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
-        DINA30, DINA38
-    };
-    uint_fast16_t elements, kk;
-    inv_error_t result;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4);
-    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT);
-
-    if (accuracy & INV_16_BIT) {
-        regs[0] = DINAF8 + 2;
-    }
-
-    fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy;
-
-    for (kk = 0; kk < 4; ++kk) {
-        if ((elements & 1) == 0)
-            regs[kk + 1] = DINAA0 + 3;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_8, 5, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-/** Sends raw data to the FIFO. 
- *  Should be called once after inv_dmp_open() and before inv_dmp_start().
- *  @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them
- *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together
- *            for a subset. The first element is temperature, the next 3 are gyro data,
- *            and the last 3 accel data.
- *  @param  accuracy
- *              The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off.
- *  @return 0 if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    int result;
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-    unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
-        DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
-        DINAA0 + 3
-    };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (accuracy)
-        accuracy = INV_16_BIT;
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
-
-    if (elements & 1)
-        fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
-    else
-        fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
-    if (elements & 0x7e)
-        fifo_obj.data_config[CONFIG_RAW_DATA] =
-            (0x3f & (elements >> 1)) | INV_16_BIT;
-    else
-        fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
-
-    if (elements & INV_ELEMENT_1) {
-        regs[0] = DINACA;
-    }
-    if (elements & INV_ELEMENT_2) {
-        regs[1] = DINBC4;
-    }
-    if (elements & INV_ELEMENT_3) {
-        regs[2] = DINACC;
-    }
-    if (elements & INV_ELEMENT_4) {
-        regs[3] = DINBC6;
-    }
-    if (elements & INV_ELEMENT_5) {
-        regs[4] = DINBC0;
-    }
-    if (elements & INV_ELEMENT_6) {
-        regs[5] = DINAC8;
-    }
-    if (elements & INV_ELEMENT_7) {
-        regs[6] = DINBC2;
-    }
-    result = inv_set_mpu_memory(KEY_CFG_15, 7, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-
-#else
-    INVENSENSE_FUNC_START;
-    unsigned char regs[4] = { DINAA0 + 3,
-        DINAA0 + 3,
-        DINAA0 + 3,
-        DINAA0 + 3
-    };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (accuracy)
-        accuracy = INV_16_BIT;
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
-
-    if (elements & 0x03) {
-        elements |= 0x03;
-        regs[0] = DINA20;
-    }
-    if (elements & 0x0C) {
-        elements |= 0x0C;
-        regs[1] = DINA28;
-    }
-    if (elements & 0x30) {
-        elements |= 0x30;
-        regs[2] = DINA30;
-    }
-    if (elements & 0x40) {
-        elements |= 0xC0;
-        regs[3] = DINA38;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_15, 4, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    if (elements & 0x01)
-        fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
-    else
-        fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
-    if (elements & 0xfe)
-        fifo_obj.data_config[CONFIG_RAW_DATA] =
-            (0x7f & (elements >> 1)) | INV_16_BIT;
-    else
-        fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
-
-    return inv_set_footer();
-#endif
-}
-
-/** Sends raw external data to the FIFO. 
- *  Should be called once after inv_dmp_open() and before inv_dmp_start().
- *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
- *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- *            for a subset. 
- *  @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data.
- *            Sending and Stop sending are reference counted, so data actually
- *            stops when the reference reaches zero.
- */
-inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
-                                          uint_fast16_t accuracy)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-    int result;
-    unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3,
-                              DINAA0 + 3, DINAA0 + 3,
-                              DINAA0 + 3, DINAA0 + 3 };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (accuracy)
-        accuracy = INV_16_BIT;
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6);
-
-    if (elements)
-        fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT;
-    else
-        fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0;
-
-    if (elements & INV_ELEMENT_1) {
-        regs[0] = DINBC2;
-    }
-    if (elements & INV_ELEMENT_2) {
-        regs[1] = DINACA;
-    }
-    if (elements & INV_ELEMENT_3) {
-        regs[2] = DINBC4;
-    }
-    if (elements & INV_ELEMENT_4) {
-        regs[3] = DINBC0;
-    }
-    if (elements & INV_ELEMENT_5) {
-        regs[4] = DINAC8;
-    }
-    if (elements & INV_ELEMENT_6) {
-        regs[5] = DINACC;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-
-#else
-    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;    // Feature not supported
-#endif
-}
-
-/**
- *  @brief  Send the Quantized Acceleromter data into the FIFO.  The data can be
- *          retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel().
- *
- *  To be useful this should be set to fifo_rate + 1 if less than
- *  INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work.
- *
- *  @param  elements
- *              the components bitmask.
- *              To send all compoents use INV_ALL.
- *
- *  @param  accuracy
- *              Use INV_32_BIT for 32-bit data or INV_16_BIT for 
- *              16-bit data.
- *              Set to zero to remove it from the FIFO.
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
-                                     uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
-        DINA30, DINA38
-    };
-    unsigned char regs2[4] = { DINA20, DINA28,
-        DINA30, DINA38
-    };
-    inv_error_t result;
-    int_fast8_t kk;
-    int_fast8_t ii;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8);
-
-    if (elements) {
-        fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT;
-    } else {
-        fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0;
-    }
-
-    for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) {
-        fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0;
-        for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
-            fifo_obj.decoded_accel[kk][ii] = 0;
-        }
-    }
-
-    for (kk = 0; kk < 4; ++kk) {
-        if ((elements & 1) == 0)
-            regs[kk + 1] = DINAA0 + 3;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    for (kk = 0; kk < 4; ++kk) {
-        if ((elements & 1) == 0)
-            regs2[kk] = DINAA0 + 3;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return inv_set_footer();
-}
-
-inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy)
-{
-    INVENSENSE_FUNC_START;
-    int_fast8_t kk;
-    unsigned char regs[3] = { DINA28, DINA30, DINA38 };
-    inv_error_t result;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (accuracy) {
-        accuracy = INV_32_BIT;
-    }
-
-    elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3);
-    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS);
-
-    fifo_obj.data_config[CONFIG_EIS] = elements | accuracy;
-
-    for (kk = 0; kk < 3; ++kk) {
-        if ((elements & 1) == 0)
-            regs[kk] = DINAA0 + 7;
-        elements >>= 1;
-    }
-
-    result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs);
-
-    return inv_set_footer();
-}
-
-/** 
- * @brief       Returns 3-element vector of accelerometer data in body frame.
- *
- * @param[out]  data    3-element vector of accelerometer data in body frame.
- *                      One gee = 2^16.
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_accel(long *data)
-{
-    int kk;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if ((!fifo_obj.data_config[CONFIG_ACCEL] &&
-         (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR))
-        ||
-        (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
-         !inv_accel_present()))
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_ACCEL + kk];
-    }
-
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 4-element quaternion vector derived from 6-axis or 
- *  9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is
- *  gyros, accel and compass.
- *
- *  @param[out] data    4-element quaternion vector. One is scaled to 2^30.
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_quaternion(long *data)
-{
-    int kk;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_QUAT])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < 4; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_QUATERNION + kk];
-    }
-
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 4-element quaternion vector derived from 6 
- *              axis sensors (gyros and accels).
- *  @param[out] data    
- *                  4-element quaternion vector. One is scaled to 2^30.
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_6axis_quaternion(long *data)
-{
-    int kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_QUAT])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < 4; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk];
-    }
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_get_relative_quaternion(long *data)
-{
-    if (data == NULL)
-        return INV_ERROR;
-    data[0] = inv_obj.relative_quat[0];
-    data[1] = inv_obj.relative_quat[1];
-    data[2] = inv_obj.relative_quat[2];
-    data[3] = inv_obj.relative_quat[3];
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief  Returns 3-element vector of gyro data in body frame.
- *  @param[out] data    
- *              3-element vector of gyro data in body frame 
- *              with gravity removed. One degree per second = 2^16.
- *  @return 0 on success or an error code.
- */
-inv_error_t inv_get_gyro(long *data)
-{
-    int kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (fifo_obj.data_config[CONFIG_GYROS]) {
-        for (kk = 0; kk < 3; ++kk) {
-            data[kk] = fifo_obj.decoded[REF_GYROS + kk];
-        }
-        return INV_SUCCESS;
-    } else {
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-    }
-}
-
-/**
- *  @brief  Get the 3-element gravity vector from the FIFO expressed
- *          in coordinates relative to the body frame.
- *  @param  data    
- *              3-element vector of gravity in body frame.
- *  @return 0 on success or an error code.
- */
-inv_error_t inv_get_gravity(long *data)
-{
-    long quat[4];
-    int ii;
-    inv_error_t result;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_QUAT])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) {
-        fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY;
-
-        // Compute it from Quaternion
-        result = inv_get_quaternion(quat);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        data[0] =
-            inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
-        data[1] =
-            inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
-        data[2] =
-            (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) -
-            1073741824L;
-
-        for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
-            data[ii] >>= 14;
-            fifo_obj.gravity_cache[ii] = data[ii];
-        }
-    } else {
-        data[0] = fifo_obj.gravity_cache[0];
-        data[1] = fifo_obj.gravity_cache[1];
-        data[2] = fifo_obj.gravity_cache[2];
-    }
-
-    return INV_SUCCESS;
-}
-
-/** 
-* @brief        Sets the filter coefficent used for computing the acceleration
-*               bias which is used to compute linear acceleration.
-* @param[in] coef   Fitler coefficient. 0. means no filter, a small number means
-*                   a small cutoff frequency with an increasing number meaning 
-*                   an increasing cutoff frequency.
-*/
-inv_error_t inv_set_linear_accel_filter_coef(float coef)
-{
-    fifo_obj.acc_filter_coef = coef;
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 3-element vector of accelerometer data in body frame
- *              with gravity removed.
- *  @param[out] data    3-element vector of accelerometer data in body frame
- *                      with gravity removed. One g = 2^16.
- *  @return     0 on success or an error code. data unchanged on error.
- */
-inv_error_t inv_get_linear_accel(long *data)
-{
-    int kk;
-    long grav[3];
-    long la[3];
-    inv_error_t result;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    result = inv_get_gravity(grav);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_get_accel(la);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) {
-        fifo_obj.cache |= FIFO_CACHE_ACC_BIAS;
-
-        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-            long x;
-            x = la[kk] - grav[kk];
-            fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef +
-                                                fifo_obj.acc_bias_filt[kk] *
-                                                (1.f -
-                                                 fifo_obj.acc_filter_coef));
-            data[kk] = x - fifo_obj.acc_bias_filt[kk];
-        }
-    } else {
-        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-            data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk];
-        }
-    }
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 3-element vector of accelerometer data in world frame
- *              with gravity removed.
- *  @param[out] data    3-element vector of accelerometer data in world frame
- *                      with gravity removed. One g = 2^16.
- *  @return     0 on success or an error code.
- */
-inv_error_t inv_get_linear_accel_in_world(long *data)
-{
-    int kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-    if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) {
-        long wtemp[4], qi[4], wtemp2[4];
-        wtemp[0] = 0;
-        inv_get_linear_accel(&wtemp[1]);
-        inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2);
-        inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi);
-        inv_q_mult(wtemp2, qi, wtemp);
-        for (kk = 0; kk < 3; ++kk) {
-            data[kk] = wtemp[kk + 1];
-        }
-        return INV_SUCCESS;
-    } else {
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-    }
-}
-
-/**
- *  @brief      Returns 4-element vector of control data.
- *  @param[out] data    4-element vector of control data.
- *  @return     0 for succes or an error code.
- */
-inv_error_t inv_get_cntrl_data(long *data)
-{
-    int kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_CONTROL_DATA])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < 4; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_CONTROL + kk];
-    }
-
-    return INV_SUCCESS;
-
-}
-
-/**
- *  @brief      Returns 3-element vector of EIS shfit data
- *  @param[out] data    3-element vector of EIS shift data.
- *  @return     0 for succes or an error code.
- */
-inv_error_t inv_get_eis(long *data)
-{
-    int kk;
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_EIS])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < 3; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_EIS + kk];
-    }
-
-    return INV_SUCCESS;
-
-}
-
-/** 
- *  @brief      Returns 3-element vector of accelerometer data in body frame.
- *  @param[out] data    3-element vector of accelerometer data in body frame in g's.
- *  @return     0 for success or an error code.
- */
-inv_error_t inv_get_accel_float(float *data)
-{
-    long lData[3];
-    int kk;
-    int result;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    result = inv_get_accel(lData);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
-        data[kk] = lData[kk] / 65536.0f;
-    }
-
-    return INV_SUCCESS;
-}
-
-/** 
- *  @brief      Returns 4-element quaternion vector.
- *  @param[out] data    4-element quaternion vector.
- *  @return     0 on success, an error code otherwise.
- */
-inv_error_t inv_get_quaternion_float(float *data)
-{
-    int kk;
-
-    if (data == NULL)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    if (!fifo_obj.data_config[CONFIG_QUAT])
-        return INV_ERROR_FEATURE_NOT_ENABLED;
-
-    for (kk = 0; kk < 4; ++kk) {
-        data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief   Command the MPU to put data in the FIFO at a particular rate.
- *
- *          The DMP will add fifo entries every fifoRate + 1 MPU cycles.  For
- *          example if the MPU is running at 200Hz the following values apply:
- *
- *          <TABLE>
- *          <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR>
- *          <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR>
- *          <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR>
- *          <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR>
- *          <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR>
- *          <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR>
- *          <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR>
- *          </TABLE>
- *
- *          Note: if the DMP is running, (state == INV_STATE_DMP_STARTED)
- *          then inv_run_state_callbacks() will be called to allow features
- *          that depend upon fundamental constants to be updated.
- *
- *  @pre    inv_dmp_open() 
- *          @ifnot MPL_MF 
- *              or inv_open_low_power_pedometer() 
- *              or inv_eis_open_dmp()
- *          @endif
- *          and inv_dmp_start() 
- *          must <b>NOT</b> have been called.
- *
- * @param   fifoRate    Divider value - 1.  Output rate is 
- *          (DMP Sample Rate) / (fifoRate + 1).
- *
- * @return  INV_SUCCESS if successful, ML error code on any failure.
- */
-inv_error_t inv_set_fifo_rate(unsigned short fifoRate)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[2];
-    unsigned char state;
-    inv_error_t result = INV_SUCCESS;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    state = inv_get_state();
-    if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    fifo_obj.fifo_rate = fifoRate;
-
-    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
-
-        regs[0] = (unsigned char)((fifoRate >> 8) & 0xff);
-        regs[1] = (unsigned char)(fifoRate & 0xff);
-
-        result = inv_set_mpu_memory(KEY_D_0_22, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        fifo_obj.sample_step_size_ms = 
-            (unsigned short)(((long)fifoRate + 1) *
-                             (inv_mpu_get_sampling_period_us
-                              (mldl_cfg)) / 1000L);
-
-        if (state == INV_STATE_DMP_STARTED)
-            result = inv_run_state_callbacks(state);
-    } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) {
-        struct ext_slave_config config;
-        long data;
-        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
-        config.len = sizeof(long);
-        config.apply = (state == INV_STATE_DMP_STARTED);
-        config.data = &data;
-        data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1);
-
-        /* Ask for the same frequency */
-        result = inv_mpu_config_accel(mldl_cfg,
-                                      inv_get_serial_handle(),
-                                      inv_get_serial_handle(), &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_mpu_get_accel_config(mldl_cfg,
-                                          inv_get_serial_handle(),
-                                          inv_get_serial_handle(), &config);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        if(FIFO_DEBUG)
-            MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000);
-        /* Record the actual frequency granted odr is in mHz */
-        fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data);
-    }
-    return result;
-}
-
-/**
- * @brief   Retrieve the current FIFO update divider - 1.
- *          See inv_set_fifo_rate() for how this value is used.
- *
- *          The fifo rate when there is no fifo is the equivilent divider when
- *          derived from the value set by SetSampleSteSizeMs()
- *          
- * @return  The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error.
- */
-unsigned short inv_get_fifo_rate(void)
-{
-    return fifo_obj.fifo_rate;
-}
-
-/**
- * @brief   Returns the step size for quaternion type data.
- *
- * Typically the data rate for each FIFO packet. When the gryos are sleeping
- * this value will return the last value set by SetSampleStepSizeMs()
- *
- * @return  step size for quaternion type data
- */
-int_fast16_t inv_get_sample_step_size_ms(void)
-{
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
-        return (fifo_obj.fifo_rate + 1) *
-            (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000);
-    else
-        return fifo_obj.sample_step_size_ms;
-}
-
-/**
- * @brief   Returns the step size for quaternion type data.
- *
- * Typically the data rate for each FIFO packet. When the gryos are sleeping
- * this value will return the last value set by SetSampleStepSizeMs()
- *
- * @return  step size for quaternion type data
- */
-int_fast16_t inv_get_sample_frequency(void)
-{
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
-        return (inv_mpu_get_sampling_rate_hz(mldl_cfg) /
-                (fifo_obj.fifo_rate + 1));
-    else
-        return (1000 / fifo_obj.sample_step_size_ms);
-}
-
-/** 
- *  @brief  The gyro data magnitude squared : 
- *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
- *  @return the computed magnitude squared output of the gyroscope.
- */
-unsigned long inv_get_gyro_sum_of_sqr(void)
-{
-    unsigned long gmag = 0;
-    long temp;
-    int kk;
-
-    for (kk = 0; kk < 3; ++kk) {
-        temp = fifo_obj.decoded[REF_GYROS + kk] >>
-            (16 - (GYRO_MAG_SQR_SHIFT / 2));
-        gmag += temp * temp;
-    }
-
-    return gmag;
-}
-
-/** 
- *  @brief  The gyro data magnitude squared:
- *          (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT.
- *  @return the computed magnitude squared output of the accelerometer.
- */
-unsigned long inv_accel_sum_of_sqr(void)
-{
-    unsigned long amag = 0;
-    long temp;
-    int kk;
-    long accel[3];
-    inv_error_t result;
-
-    result = inv_get_accel(accel);
-    if (INV_SUCCESS != result) {
-        return 0;
-    }
-
-    for (kk = 0; kk < 3; ++kk) {
-        temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2));
-        amag += temp * temp;
-    }
-    return amag;
-}
-
-/**
- *  @internal
- */
-void inv_override_quaternion(float *q)
-{
-    int kk;
-    for (kk = 0; kk < 4; ++kk) {
-        fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30));
-    }
-}
-
-/**
- * @internal
- * @brief   This registers a function to be called for each set of 
- *          gyro/quaternion/rotation matrix/etc output.
- * @param[in] func The callback function to register
- * @param[in] priority The unique priority number of the callback. Lower numbers
- *            are called first.
- * @return  error code.
- */
-inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    int kk, nn;
-
-    result = inv_lock_mutex(fifo_rate_obj.mutex);
-    if (INV_SUCCESS != result) {
-        return result;
-    }
-    // Make sure we haven't registered this function already
-    // Or used the same priority
-    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
-        if ((fifo_rate_obj.fifo_process_cb[kk] == func) ||
-            (fifo_rate_obj.priority[kk] == priority)) {
-            inv_unlock_mutex(fifo_rate_obj.mutex);
-            return INV_ERROR_INVALID_PARAMETER;
-        }
-    }
-
-    // Make sure we have not filled up our number of allowable callbacks
-    if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) {
-        kk = 0;
-        if (fifo_rate_obj.num_cb != 0) {
-            // set kk to be where this new callback goes in the array
-            while ((kk < fifo_rate_obj.num_cb) &&
-                   (fifo_rate_obj.priority[kk] < priority)) {
-                kk++;
-            }
-            if (kk != fifo_rate_obj.num_cb) {
-                // We need to move the others
-                for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) {
-                    fifo_rate_obj.fifo_process_cb[nn] =
-                        fifo_rate_obj.fifo_process_cb[nn - 1];
-                    fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1];
-                }
-            }
-        }
-        // Add new callback
-        fifo_rate_obj.fifo_process_cb[kk] = func;
-        fifo_rate_obj.priority[kk] = priority;
-        fifo_rate_obj.num_cb++;
-    } else {
-        result = INV_ERROR_MEMORY_EXAUSTED;
-    }
-
-    inv_unlock_mutex(fifo_rate_obj.mutex);
-    return result;
-}
-
-/**
- * @internal
- * @brief   This unregisters a function to be called for each set of 
- *          gyro/quaternion/rotation matrix/etc output.
- * @return  error code.
- */
-inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func)
-{
-    INVENSENSE_FUNC_START;
-    int kk, jj;
-    inv_error_t result;
-
-    result = inv_lock_mutex(fifo_rate_obj.mutex);
-    if (INV_SUCCESS != result) {
-        return result;
-    }
-    // Make sure we haven't registered this function already
-    result = INV_ERROR_INVALID_PARAMETER;
-    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
-        if (fifo_rate_obj.fifo_process_cb[kk] == func) {
-            for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) {
-                fifo_rate_obj.fifo_process_cb[jj - 1] =
-                    fifo_rate_obj.fifo_process_cb[jj];
-                fifo_rate_obj.priority[jj - 1] =
-                    fifo_rate_obj.priority[jj];
-            }
-            fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL;
-            fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0;
-            fifo_rate_obj.num_cb--;
-            result = INV_SUCCESS;
-            break;
-        }
-    }
-
-    inv_unlock_mutex(fifo_rate_obj.mutex);
-    return result;
-
-}
-#ifdef UMPL
-bool bFIFIDataAvailable = FALSE;
-bool isUmplDataInFIFO(void)
-{
-    return bFIFIDataAvailable;
-}
-void setUmplDataInFIFOFlag(bool flag)
-{
-    bFIFIDataAvailable = flag;
-}
-#endif
-inv_error_t inv_run_fifo_rate_processes(void)
-{
-    int kk;
-    inv_error_t result, result2;
-
-    result = inv_lock_mutex(fifo_rate_obj.mutex);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("MLOsLockMutex returned %d\n", result);
-        return result;
-    }
-    // User callbacks take priority over the fifo_process_cb callback
-    if (fifo_obj.fifo_process_cb)
-        fifo_obj.fifo_process_cb();
-
-    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
-        if (fifo_rate_obj.fifo_process_cb[kk]) {
-            result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj);
-            if (result == INV_SUCCESS)
-#ifdef UMPL		 
-	 setUmplDataInFIFOFlag(TRUE);
-#endif
-                result = result2;
-        }
-    }
-
-    inv_unlock_mutex(fifo_rate_obj.mutex);
-    return result;
-}
-
-/*********************/
-         /** \}*//* defgroup */
-/*********************/
diff --git a/mlsdk/mllite/mlFIFO.h b/mlsdk/mllite/mlFIFO.h
deleted file mode 100644
index 2114eb3..0000000
--- a/mlsdk/mllite/mlFIFO.h
+++ /dev/null
@@ -1,150 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 INVENSENSE_INV_FIFO_H__
-#define INVENSENSE_INV_FIFO_H__
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "ml.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlFIFO_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    /**************************************************************************/
-    /*  Elements                                                              */
-    /**************************************************************************/
-
-#define INV_ELEMENT_1                    (0x0001)
-#define INV_ELEMENT_2                    (0x0002)
-#define INV_ELEMENT_3                    (0x0004)
-#define INV_ELEMENT_4                    (0x0008)
-#define INV_ELEMENT_5                    (0x0010)
-#define INV_ELEMENT_6                    (0x0020)
-#define INV_ELEMENT_7                    (0x0040)
-#define INV_ELEMENT_8                    (0x0080)
-
-#define INV_ALL                          (0xFFFF)
-#define INV_ELEMENT_MASK                 (0x00FF)
-
-    /**************************************************************************/
-    /*  Accuracy                                                              */
-    /**************************************************************************/
-
-#define INV_16_BIT                       (0x0100)
-#define INV_32_BIT                       (0x0200)
-#define INV_ACCURACY_MASK                (0x0300)
-
-    /**************************************************************************/
-    /*  Accuracy                                                              */
-    /**************************************************************************/
-
-#define INV_GYRO_FROM_RAW                (0x00)
-#define INV_GYRO_FROM_QUATERNION         (0x01)
-
-    /**************************************************************************/
-    /*  High Rate Proceses                                                    */
-    /**************************************************************************/
-
-#define MAX_HIGH_RATE_PROCESSES 16
-
-    /**************************************************************************/
-    /*  Prototypes                                                            */
-    /**************************************************************************/
-
-    inv_error_t inv_set_fifo_rate(unsigned short fifoRate);
-    unsigned short inv_get_fifo_rate(void);
-    int_fast16_t inv_get_sample_step_size_ms(void);
-    int_fast16_t inv_get_sample_frequency(void);
-    long inv_decode_temperature(short tempReg);
-
-    // Register callbacks after a packet of FIFO data is processed
-    inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority);
-    inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func);
-    inv_error_t inv_run_fifo_rate_processes(void);
-
-    // Setup FIFO for various output
-    inv_error_t inv_send_quaternion(uint_fast16_t accuracy);
-    inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy);
-    inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy);
-    inv_error_t inv_send_linear_accel(uint_fast16_t elements,
-                                      uint_fast16_t accuracy);
-    inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
-                                               uint_fast16_t accuracy);
-    inv_error_t inv_send_cntrl_data(uint_fast16_t elements,
-                                    uint_fast16_t accuracy);
-    inv_error_t inv_send_sensor_data(uint_fast16_t elements,
-                                     uint_fast16_t accuracy);
-    inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
-                                              uint_fast16_t accuracy);
-    inv_error_t inv_send_gravity(uint_fast16_t elements,
-                                 uint_fast16_t accuracy);
-    inv_error_t inv_send_packet_number(uint_fast16_t accuracy);
-    inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
-                                         uint_fast16_t accuracy);
-    inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy);
-
-    // Get Fixed Point data from FIFO
-    inv_error_t inv_get_accel(long *data);
-    inv_error_t inv_get_quaternion(long *data);
-    inv_error_t inv_get_6axis_quaternion(long *data);
-    inv_error_t inv_get_relative_quaternion(long *data);
-    inv_error_t inv_get_gyro(long *data);
-    inv_error_t inv_set_linear_accel_filter_coef(float coef);
-    inv_error_t inv_get_linear_accel(long *data);
-    inv_error_t inv_get_linear_accel_in_world(long *data);
-    inv_error_t inv_get_gyro_and_accel_sensor(long *data);
-    inv_error_t inv_get_gyro_sensor(long *data);
-    inv_error_t inv_get_cntrl_data(long *data);
-    inv_error_t inv_get_temperature(long *data);
-    inv_error_t inv_get_gravity(long *data);
-    inv_error_t inv_get_unquantized_accel(long *data);
-    inv_error_t inv_get_quantized_accel(long *data);
-    inv_error_t inv_get_external_sensor_data(long *data, int size);
-    inv_error_t inv_get_eis(long *data);
-
-    // Get Floating Point data from FIFO
-    inv_error_t inv_get_accel_float(float *data);
-    inv_error_t inv_get_quaternion_float(float *data);
-
-    inv_error_t inv_process_fifo_packet(const unsigned char *dmpData);
-    inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
-                                          int_fast8_t * processed);
-
-    inv_error_t inv_set_fifo_processed_callback(void (*func) (void));
-
-    inv_error_t inv_init_fifo_param(void);
-    inv_error_t inv_close_fifo(void);
-    inv_error_t inv_set_gyro_data_source(uint_fast8_t source);
-    inv_error_t inv_decode_quantized_accel(void);
-    unsigned long inv_get_gyro_sum_of_sqr(void);
-    unsigned long inv_accel_sum_of_sqr(void);
-    void inv_override_quaternion(float *q);
-#ifdef UMPL
-    bool isUmplDataInFIFO(void);
-    void setUmplDataInFIFOFlag(bool flag);
-#endif
-    uint_fast16_t inv_get_fifo_packet_size(void);
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INVENSENSE_INV_FIFO_H__
diff --git a/mlsdk/mllite/mlFIFOHW.c b/mlsdk/mllite/mlFIFOHW.c
deleted file mode 100644
index 7a77e66..0000000
--- a/mlsdk/mllite/mlFIFOHW.c
+++ /dev/null
@@ -1,328 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *******************************************************************************/
-
-/** 
- *  @defgroup MLFIFO_HW 
- *  @brief  Motion Library - FIFO HW Driver.
- *          Provides facilities to interact with the FIFO.
- *
- *  @{
- *      @file   mlFIFOHW.c
- *      @brief  The Motion Library Fifo Hardware Layer.
- *
- */
-
-#include <string.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-#    include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-#    include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-#  include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mlFIFOHW.h"
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-
-#include "mlsl.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-fifo"
-
-/*
-    Defines
-*/
-
-#define _fifoDebug(x)           //{x}
-
-/*
-    Typedefs
-*/
-
-struct fifo_hw_obj {
-    short fifoCount;
-    inv_error_t fifoError;
-    unsigned char fifoOverflow;
-    unsigned char fifoResetOnOverflow;
-};
-
-/*
-    Global variables
-*/
-const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
-
-/*
-    Static variables
-*/
-static struct fifo_hw_obj fifo_objHW;
-
-/*
-    Definitions
-*/
-
-/**
- *  @brief  Initializes the internal FIFO data structure.
- */
-void inv_init_fifo_hardare(void)
-{
-    memset(&fifo_objHW, 0, sizeof(fifo_objHW));
-    fifo_objHW.fifoResetOnOverflow = TRUE;
-}
-
-/**
- *  @internal
- *  @brief  used to get the FIFO data.
- *  @param  length  
- *              Number of bytes to read from the FIFO.
- *  @param  buffer  
- *              the bytes of FIFO data.
- *              Note that this buffer <b>must</b> be large enough
- *              to store and additional trailing FIFO footer when 
- *              expected.  The callers must make sure enough space
- *              is allocated.
- *  @return number of valid bytes of data.
-**/
-uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    uint_fast16_t inFifo;
-    uint_fast16_t toRead;
-    int_fast8_t kk;
-
-    toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
-    /*---- make sure length is correct ----*/
-    if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
-        fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
-        return 0;
-    }
-
-    result = inv_get_fifo_length(&inFifo);
-    if (INV_SUCCESS != result) {
-        fifo_objHW.fifoError = result;
-        return 0;
-    }
-    // fifo_objHW.fifoCount is the footer size left in the buffer, or 
-    //      0 if this is the first time reading the fifo since it was reset
-    if (inFifo < length + fifo_objHW.fifoCount) {
-        fifo_objHW.fifoError = INV_SUCCESS;
-        return 0;
-    }
-    // if a trailing fifo count is expected - start storing data 2 bytes before
-    result =
-        inv_read_fifo(fifo_objHW.fifoCount >
-                      0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
-    if (INV_SUCCESS != result) {
-        fifo_objHW.fifoError = result;
-        return 0;
-    }
-    // Make sure the fifo didn't overflow before or during the read
-    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                             MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
-    if (INV_SUCCESS != result) {
-        fifo_objHW.fifoError = result;
-        return 0;
-    }
-
-    if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
-        MPL_LOGV("Resetting Fifo : Overflow\n");
-        inv_reset_fifo();
-        fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
-        return 0;
-    }
-
-    /* Check the Footer value to give us a chance at making sure data 
-     * didn't get corrupted */
-    for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
-        if (buffer[kk] != gFifoFooter[kk]) {
-            MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
-                     buffer[0], buffer[1]);
-            _fifoDebug(char out[200];
-                       MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
-                       sprintf(out, "0x");
-                       for (kk = 0; kk < (int)toRead; kk++) {
-                       sprintf(out, "%s%02X", out, buffer[kk]);}
-                       MPL_LOGW("%s\n", out);)
-                inv_reset_fifo();
-            fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
-            return 0;
-        }
-    }
-
-    if (fifo_objHW.fifoCount == 0) {
-        fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
-    }
-
-    return length - FIFO_FOOTER_SIZE;
-}
-
-/**
- *  @brief  Used to query the status of the FIFO.
- *  @return INV_SUCCESS if the fifo is OK. An error code otherwise.
-**/
-inv_error_t inv_get_fifo_status(void)
-{
-    inv_error_t fifoError = fifo_objHW.fifoError;
-    fifo_objHW.fifoError = 0;
-    return fifoError;
-}
-
-/** 
- * @internal
- * @brief   Get the length from the fifo
- * 
- * @param[out] len amount of data currently stored in the fifo.
- * 
- * @return INV_SUCCESS or non-zero error code.
-**/
-inv_error_t inv_get_fifo_length(uint_fast16_t * len)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char fifoBuf[2];
-    inv_error_t result;
-
-    if (NULL == len)
-        return INV_ERROR_INVALID_PARAMETER;
-
-    /*---- read the 2 'count' registers and 
-      burst read the data from the FIFO ----*/
-    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                             MPUREG_FIFO_COUNTH, 2, fifoBuf);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("ReadBurst failed %d\n", result);
-        inv_reset_fifo();
-        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
-        *len = 0;
-        return result;
-    }
-
-    *len = (uint_fast16_t) (fifoBuf[0] << 8);
-    *len += (uint_fast16_t) (fifoBuf[1]);
-    return result;
-}
-
-/**
- *  @brief  inv_get_fifo_count is used to get the number of bytes left in the FIFO.
- *          This function returns the stored value and does not access the hardware.  
- *          See inv_get_fifo_length().
- *  @return the number of bytes left in the FIFO
-**/
-short inv_get_fifo_count(void)
-{
-    return fifo_objHW.fifoCount;
-}
-
-/** 
- *  @internal
- *  @brief  Read data from the fifo
- * 
- *  @param[out] data Location to store the date read from the fifo
- *  @param[in] len   Amount of data to read out of the fifo
- * 
- *  @return INV_SUCCESS or non-zero error code
-**/
-inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    result = inv_serial_read_fifo(inv_get_serial_handle(),
-                                  inv_get_mpu_slave_addr(),
-                                  (unsigned short)len, data);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("inv_serial_readBurst failed %d\n", result);
-        inv_reset_fifo();
-        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
-        return result;
-    }
-    return result;
-}
-
-/**
- *  @brief  Clears the FIFO status and its content. 
- *  @note   Halt the DMP writing into the FIFO for the time 
- *          needed to reset the FIFO.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_reset_fifo(void)
-{
-    INVENSENSE_FUNC_START;
-    int len = FIFO_HW_SIZE;
-    unsigned char fifoBuf[2];
-    unsigned char tries = 0;
-    unsigned char userCtrlReg;
-    inv_error_t result;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    fifo_objHW.fifoCount = 0;
-    if (mldl_cfg->gyro_is_suspended)
-        return INV_SUCCESS;
-
-    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                             MPUREG_USER_CTRL, 1, &userCtrlReg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    while (len != 0 && tries < 6) {
-        result =
-            inv_serial_single_write(inv_get_serial_handle(),
-                                    inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
-                                    ((userCtrlReg & (~BIT_FIFO_EN)) |
-                                     BIT_FIFO_RST));
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result =
-            inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
-                            MPUREG_FIFO_COUNTH, 2, fifoBuf);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
-        tries++;
-    }
-
-    result =
-        inv_serial_single_write(inv_get_serial_handle(),
-                                inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
-                                userCtrlReg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- * @}
-**/
diff --git a/mlsdk/mllite/mlFIFOHW.h b/mlsdk/mllite/mlFIFOHW.h
deleted file mode 100644
index 6f70185..0000000
--- a/mlsdk/mllite/mlFIFOHW.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 INVENSENSE_INV_FIFO_HW_H__
-#define INVENSENSE_INV_FIFO_HW_H__
-
-#include "mpu.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlFIFOHW_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    // This is the maximum amount of FIFO data we would read in one packet
-#define MAX_FIFO_LENGTH             (256)
-    // This is the hardware size of the FIFO
-#define FIFO_FOOTER_SIZE            (2)
-
-    uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer);
-    inv_error_t inv_get_fifo_status(void);
-    inv_error_t inv_get_fifo_length(uint_fast16_t * len);
-    short inv_get_fifo_count(void);
-    inv_error_t inv_reset_fifo(void);
-    void inv_init_fifo_hardare();
-    inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INVENSENSE_INV_FIFO_HW_H__
diff --git a/mlsdk/mllite/mlMathFunc.c b/mlsdk/mllite/mlMathFunc.c
deleted file mode 100644
index 0d8e7ec..0000000
--- a/mlsdk/mllite/mlMathFunc.c
+++ /dev/null
@@ -1,377 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 "mlmath.h"
-#include "mlMathFunc.h"
-#include "mlinclude.h"
-
-/** 
- * Performs one iteration of the filter, generating a new y(0) 
- *         1     / N                /  N             \\
- * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length
- *        a(0)   \k=0               \ k=1            //
- * 
- * The filters A and B should be (sizeof(long) * state->length).
- * The state variables x and y should be (sizeof(long) * (state->length - 1))
- *
- * The state variables x and y should be initialized prior to the first call
- * 
- * @param state Contains the length of the filter, filter coefficients and
- *              filter state variables x and y.
- * @param x New input into the filter.
- */
-void inv_filter_long(struct filter_long *state, long x)
-{
-    const long *b = state->b;
-    const long *a = state->a;
-    long length = state->length;
-    long long tmp;
-    int ii;
-
-    /* filter */
-    tmp = (long long)x *(b[0]);
-    for (ii = 0; ii < length - 1; ii++) {
-        tmp += ((long long)state->x[ii] * (long long)(b[ii + 1]));
-    }
-    for (ii = 0; ii < length - 1; ii++) {
-        tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1]));
-    }
-    tmp /= (long long)a[0];
-
-    /* Delay */
-    for (ii = length - 2; ii > 0; ii--) {
-        state->y[ii] = state->y[ii - 1];
-        state->x[ii] = state->x[ii - 1];
-    }
-    /* New values */
-    state->y[0] = (long)tmp;
-    state->x[0] = x;
-}
-
-/** Performs a multiply and shift by 29. These are good functions to write in assembly on
- * with devices with small memory where you want to get rid of the long long which some
- * assemblers don't handle well
- * @param[in] a 
- * @param[in] b
- * @return ((long long)a*b)>>29
-*/
-long inv_q29_mult(long a, long b)
-{
-    long long temp;
-    long result;
-    temp = (long long)a *b;
-    result = (long)(temp >> 29);
-    return result;
-}
-
-/** Performs a multiply and shift by 30. These are good functions to write in assembly on
- * with devices with small memory where you want to get rid of the long long which some
- * assemblers don't handle well
- * @param[in] a 
- * @param[in] b
- * @return ((long long)a*b)>>30
-*/
-long inv_q30_mult(long a, long b)
-{
-    long long temp;
-    long result;
-    temp = (long long)a *b;
-    result = (long)(temp >> 30);
-    return result;
-}
-
-void inv_q_mult(const long *q1, const long *q2, long *qProd)
-{
-    INVENSENSE_FUNC_START;
-    qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] -
-                       (long long)q1[2] * q2[2] -
-                       (long long)q1[3] * q2[3]) >> 30);
-    qProd[1] =
-        (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] +
-               (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30);
-    qProd[2] =
-        (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] +
-                (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30);
-    qProd[3] =
-        (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] -
-                (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30);
-}
-
-void inv_q_add(long *q1, long *q2, long *qSum)
-{
-    INVENSENSE_FUNC_START;
-    qSum[0] = q1[0] + q2[0];
-    qSum[1] = q1[1] + q2[1];
-    qSum[2] = q1[2] + q2[2];
-    qSum[3] = q1[3] + q2[3];
-}
-
-void inv_q_normalize(long *q)
-{
-    INVENSENSE_FUNC_START;
-    double normSF = 0;
-    int i;
-    for (i = 0; i < 4; i++) {
-        normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L;
-    }
-    if (normSF > 0) {
-        normSF = 1 / sqrt(normSF);
-        for (i = 0; i < 4; i++) {
-            q[i] = (int)((double)q[i] * normSF);
-        }
-    } else {
-        q[0] = 1073741824L;
-        q[1] = 0;
-        q[2] = 0;
-        q[3] = 0;
-    }
-}
-
-void inv_q_invert(const long *q, long *qInverted)
-{
-    INVENSENSE_FUNC_START;
-    qInverted[0] = q[0];
-    qInverted[1] = -q[1];
-    qInverted[2] = -q[2];
-    qInverted[3] = -q[3];
-}
-
-void inv_q_multf(const float *q1, const float *q2, float *qProd)
-{
-    INVENSENSE_FUNC_START;
-    qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
-    qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
-    qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
-    qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
-}
-
-void inv_q_addf(float *q1, float *q2, float *qSum)
-{
-    INVENSENSE_FUNC_START;
-    qSum[0] = q1[0] + q2[0];
-    qSum[1] = q1[1] + q2[1];
-    qSum[2] = q1[2] + q2[2];
-    qSum[3] = q1[3] + q2[3];
-}
-
-void inv_q_normalizef(float *q)
-{
-    INVENSENSE_FUNC_START;
-    float normSF = 0;
-    float xHalf = 0;
-    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-    if (normSF < 2) {
-        xHalf = 0.5f * normSF;
-        normSF = normSF * (1.5f - xHalf * normSF * normSF);
-        normSF = normSF * (1.5f - xHalf * normSF * normSF);
-        normSF = normSF * (1.5f - xHalf * normSF * normSF);
-        normSF = normSF * (1.5f - xHalf * normSF * normSF);
-        q[0] *= normSF;
-        q[1] *= normSF;
-        q[2] *= normSF;
-        q[3] *= normSF;
-    } else {
-        q[0] = 1.0;
-        q[1] = 0.0;
-        q[2] = 0.0;
-        q[3] = 0.0;
-    }
-    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-}
-
-/** Performs a length 4 vector normalization with a square root.
-* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero.
-*/
-void inv_q_norm4(float *q)
-{
-    float mag;
-    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-    if (mag) {
-        q[0] /= mag;
-        q[1] /= mag;
-        q[2] /= mag;
-        q[3] /= mag;
-    } else {
-        q[0] = 1.f;
-        q[1] = 0.f;
-        q[2] = 0.f;
-        q[3] = 0.f;
-    }
-}
-
-void inv_q_invertf(const float *q, float *qInverted)
-{
-    INVENSENSE_FUNC_START;
-    qInverted[0] = q[0];
-    qInverted[1] = -q[1];
-    qInverted[2] = -q[2];
-    qInverted[3] = -q[3];
-}
-
-/**
- * Converts a quaternion to a rotation matrix.
- * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
- * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
- *             First 3 elements of the rotation matrix, represent
- *             the first row of the matrix. Rotation matrix multiplied
- *             by a 3 element column vector transform a vector from Body
- *             to World.
- */
-void inv_quaternion_to_rotation(const long *quat, long *rot)
-{
-    rot[0] =
-        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
-                                                      quat[0]) - 1073741824L;
-    rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
-    rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
-    rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
-    rot[4] =
-        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
-                                                      quat[0]) - 1073741824L;
-    rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
-    rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
-    rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
-    rot[8] =
-        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
-                                                      quat[0]) - 1073741824L;
-}
-
-/** Converts a 32-bit long to a big endian byte stream */
-unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
-{
-    big8[0] = (unsigned char)((x >> 24) & 0xff);
-    big8[1] = (unsigned char)((x >> 16) & 0xff);
-    big8[2] = (unsigned char)((x >> 8) & 0xff);
-    big8[3] = (unsigned char)(x & 0xff);
-    return big8;
-}
-
-/** Converts a big endian byte stream into a 32-bit long */
-long inv_big8_to_int32(const unsigned char *big8)
-{
-    long x;
-    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) |
-        ((long)big8[3]);
-    return x;
-}
-
-/** Converts a 16-bit short to a big endian byte stream */
-unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
-{
-    big8[0] = (unsigned char)((x >> 8) & 0xff);
-    big8[1] = (unsigned char)(x & 0xff);
-    return big8;
-}
-
-void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
-{
-    int k, l, i, j;
-    for (i = 0, k = 0; i < *n; i++, k++) {
-        for (j = 0, l = 0; j < *n; j++, l++) {
-            if (i == x)
-                i++;
-            if (j == y)
-                j++;
-            *(b + 10 * k + l) = *(a + 10 * i + j);
-        }
-    }
-    *n = *n - 1;
-}
-
-void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
-{
-    int k, l, i, j;
-    for (i = 0, k = 0; i < *n; i++, k++) {
-        for (j = 0, l = 0; j < *n; j++, l++) {
-            if (i == x)
-                i++;
-            if (j == y)
-                j++;
-            *(b + 10 * k + l) = *(a + 10 * i + j);
-        }
-    }
-    *n = *n - 1;
-}
-
-float inv_matrix_det(float *p, int *n)
-{
-    float d[10][10], sum = 0;
-    int i, j, m;
-    m = *n;
-    if (*n == 2)
-        return (*p ** (p + 11) - *(p + 1) ** (p + 10));
-    for (i = 0, j = 0; j < m; j++) {
-        *n = m;
-        inv_matrix_det_inc(p, &d[0][0], n, i, j);
-        sum =
-            sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0],
-                                                                    n);
-    }
-
-    return (sum);
-}
-
-double inv_matrix_detd(double *p, int *n)
-{
-    double d[10][10], sum = 0;
-    int i, j, m;
-    m = *n;
-    if (*n == 2)
-        return (*p ** (p + 11) - *(p + 1) ** (p + 10));
-    for (i = 0, j = 0; j < m; j++) {
-        *n = m;
-        inv_matrix_det_incd(p, &d[0][0], n, i, j);
-        sum =
-            sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0],
-                                                                     n);
-    }
-
-    return (sum);
-}
-
-/** Wraps angle from (-M_PI,M_PI]
- * @param[in] ang Angle in radians to wrap
- * @return Wrapped angle from (-M_PI,M_PI]
- */
-float inv_wrap_angle(float ang)
-{
-    if (ang > M_PI)
-        return ang - 2 * (float)M_PI;
-    else if (ang <= -(float)M_PI)
-        return ang + 2 * (float)M_PI;
-    else
-        return ang;
-}
-
-/** Finds the minimum angle difference ang1-ang2 such that difference
- * is between [-M_PI,M_PI]
- * @param[in] ang1 
- * @param[in] ang2
- * @return angle difference ang1-ang2
- */
-float inv_angle_diff(float ang1, float ang2)
-{
-    float d;
-    ang1 = inv_wrap_angle(ang1);
-    ang2 = inv_wrap_angle(ang2);
-    d = ang1 - ang2;
-    if (d > M_PI)
-        d -= 2 * (float)M_PI;
-    else if (d < -(float)M_PI)
-        d += 2 * (float)M_PI;
-    return d;
-}
diff --git a/mlsdk/mllite/mlMathFunc.h b/mlsdk/mllite/mlMathFunc.h
deleted file mode 100644
index 70fa9f4..0000000
--- a/mlsdk/mllite/mlMathFunc.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 INVENSENSE_INV_MATH_FUNC_H__
-#define INVENSENSE_INV_MATH_FUNC_H__
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlMathFunc_legacy.h"
-#endif
-
-#define NUM_ROTATION_MATRIX_ELEMENTS (9)
-#define ROT_MATRIX_SCALE_LONG  (1073741824)
-#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f)
-#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \
-    ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT ))
-#define SIGNM(k)((int)(k)&1?-1:1)
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-    struct filter_long {
-        int length;
-        const long *b;
-        const long *a;
-        long *x;
-        long *y;
-    };
-
-    void inv_filter_long(struct filter_long *state, long x);
-    long inv_q29_mult(long a, long b);
-    long inv_q30_mult(long a, long b);
-    void inv_q_mult(const long *q1, const long *q2, long *qProd);
-    void inv_q_add(long *q1, long *q2, long *qSum);
-    void inv_q_normalize(long *q);
-    void inv_q_invert(const long *q, long *qInverted);
-    void inv_q_multf(const float *q1, const float *q2, float *qProd);
-    void inv_q_addf(float *q1, float *q2, float *qSum);
-    void inv_q_normalizef(float *q);
-    void inv_q_norm4(float *q);
-    void inv_q_invertf(const float *q, float *qInverted);
-    void inv_quaternion_to_rotation(const long *quat, long *rot);
-    unsigned char *inv_int32_to_big8(long x, unsigned char *big8);
-    long inv_big8_to_int32(const unsigned char *big8);
-    unsigned char *inv_int16_to_big8(short x, unsigned char *big8);
-    float inv_matrix_det(float *p, int *n);
-    void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y);
-    double inv_matrix_detd(double *p, int *n);
-    void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y);
-    float inv_wrap_angle(float ang);
-    float inv_angle_diff(float ang1, float ang2);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/mlsdk/mllite/mlSetGyroBias.c b/mlsdk/mllite/mlSetGyroBias.c
deleted file mode 100644
index bd14d2e..0000000
--- a/mlsdk/mllite/mlSetGyroBias.c
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id:$
- *
- *****************************************************************************/
-
-#include "mlSetGyroBias.h"
-#include "mlFIFO.h"
-#include "ml.h"
-#include <string.h>
-#include "mldl.h"
-#include "mlMathFunc.h"
-
-typedef struct {
-    int needToSetBias;
-    short currentBias[3];
-    int mode;
-    int motion;
-} tSGB;
-
-tSGB sgb;
-
-/** Records a motion event that may cause a callback when the priority for this
- * feature is met.
- */
-void inv_set_motion_state(int motion)
-{
-    sgb.motion = motion;
-}
-
-/** Converts from internal DMP gyro bias registers to external hardware gyro bias by
-* applying scaling and transformation.
-*/
-void inv_convert_bias(const unsigned char *regs, short *bias)
-{
-    long biasTmp2[3], biasTmp[3], biasPrev[3];
-    int i;
-    int sf;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (mldl_cfg->gyro_sens_trim != 0) {
-        sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
-    } else {
-        sf = 2000;
-    }
-    for (i = 0; i < 3; i++) {
-        biasTmp2[i] = inv_big8_to_int32(&regs[i * 4]);
-    }
-    // Rotate bias vector by the transpose of the orientation matrix
-    for (i = 0; i < 3; ++i) {
-        biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) +
-            inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) +
-            inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]);
-    }
-
-    for (i = 0; i < GYRO_NUM_AXES; i++) {
-        biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf);
-        biasPrev[i] = (long)mldl_cfg->offset[i];
-        if (biasPrev[i] > 32767)
-            biasPrev[i] -= 65536L;
-    }
-
-    for (i = 0; i < GYRO_NUM_AXES; i++) {
-        bias[i] = (short)(biasPrev[i] - biasTmp[i]);
-    }
-}
-
-/** Records hardware biases in format as used by hardware gyro registers.
-* Note, the hardware will add this value to the measured gyro data.
-*/
-inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode)
-{
-    if (sgb.currentBias[0] != bias[0])
-        sgb.needToSetBias = 1;
-    if (sgb.currentBias[1] != bias[1])
-        sgb.needToSetBias = 1;
-    if (sgb.currentBias[2] != bias[2])
-        sgb.needToSetBias = 1;
-    if (sgb.needToSetBias) {
-        memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias));
-        sgb.mode = mode;
-    }
-    return INV_SUCCESS;
-}
-
-/** Records gyro biases
-* @param[in] bias Bias where 1dps is 2^16. In chip frame.
-*/
-inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode)
-{
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    int sf, i;
-    long biasTmp;
-    short offset[3];
-    inv_error_t result;
-
-    if (mldl_cfg->gyro_sens_trim != 0) {
-        sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
-    } else {
-        sf = 2000;
-    }
-
-    for (i = 0; i < GYRO_NUM_AXES; i++) {
-        biasTmp = -bias[i] / sf;
-        if (biasTmp < 0)
-            biasTmp += 65536L;
-        offset[i] = (short)biasTmp;
-    }
-    result = inv_set_gyro_bias_in_hw_unit(offset, mode);
-    return result;
-}
-
-inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode)
-{
-    long biasL[3];
-    inv_error_t result;
-
-    biasL[0] = (long)(bias[0] * (1L << 16));
-    biasL[1] = (long)(bias[1] * (1L << 16));
-    biasL[2] = (long)(bias[2] * (1L << 16));
-    result = inv_set_gyro_bias_in_dps(biasL, mode);
-    return result;
-}
-
-inv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (sgb.needToSetBias) {
-        result = inv_set_offset(sgb.currentBias);
-        sgb.needToSetBias = 0;
-    }
-
-    // Check if motion state has changed
-    if (sgb.motion == INV_MOTION) {
-        // We are moving
-        if (inv_obj->motion_state == INV_NO_MOTION) {
-            //Trigger motion callback
-            inv_obj->motion_state = INV_MOTION;
-            inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
-            if (inv_params_obj.motion_cb_func) {
-                inv_params_obj.motion_cb_func(INV_MOTION);
-            }
-        }
-    } else if (sgb.motion == INV_NO_MOTION){
-        // We are not moving
-        if (inv_obj->motion_state == INV_MOTION) {
-            //Trigger no motion callback
-            inv_obj->motion_state = INV_NO_MOTION;
-            inv_obj->got_no_motion_bias = TRUE;
-            inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION;
-            if (inv_params_obj.motion_cb_func) {
-                inv_params_obj.motion_cb_func(INV_NO_MOTION);
-            }
-        }
-    }
-
-    return result;
-}
-
-inv_error_t inv_enable_set_bias(void)
-{
-    inv_error_t result;
-    memset(&sgb, 0, sizeof(sgb));
-
-    sgb.motion = inv_obj.motion_state;
-
-    result =
-        inv_register_fifo_rate_process(MLSetGyroBiasCB,
-                                       INV_PRIORITY_SET_GYRO_BIASES);
-    if (result == INV_ERROR_INVALID_PARAMETER)
-        result = INV_SUCCESS;    /* We already registered this */
-    return result;
-}
-
-inv_error_t inv_disable_set_bias(void)
-{
-    inv_error_t result;
-    result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB);
-    return INV_SUCCESS;          // FIXME need to disable
-}
diff --git a/mlsdk/mllite/mlSetGyroBias.h b/mlsdk/mllite/mlSetGyroBias.h
deleted file mode 100644
index e56f18b..0000000
--- a/mlsdk/mllite/mlSetGyroBias.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef INV_SET_GYRO_BIAS__H__
-#define INV_SET_GYRO_BIAS__H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define INV_SGB_NO_MOTION 4
-#define INV_SGB_FAST_NO_MOTION 5
-#define INV_SGB_TEMP_COMP 6
-
-    inv_error_t inv_enable_set_bias(void);
-    inv_error_t inv_disable_set_bias(void);
-    inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode);
-    inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode);
-    inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode);
-    void inv_convert_bias(const unsigned char *regs, short *bias);
-    void inv_set_motion_state(int motion);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INV_SET_GYRO_BIAS__H__
diff --git a/mlsdk/mllite/ml_mputest.c b/mlsdk/mllite/ml_mputest.c
deleted file mode 100644
index d7fc608..0000000
--- a/mlsdk/mllite/ml_mputest.c
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: ml_mputest.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup MPU_SELF_TEST
- *  @brief  C wrapper to integrate the MPU Self Test wrapper in MPL.
- *          Provides ML name compliant naming and an additional API that
- *          automates the suspension of normal MPL operations, runs the test,
- *          and resume.
- *
- *  @{
- *      @file   ml_mputest.c
- *      @brief  C wrapper to integrate the MPU Self Test wrapper in MPL.
- *              The main logic of the test and APIs can be found in mputest.c
- */
-
-#include <stdio.h>
-#include <time.h>
-#include <string.h>
-#include <math.h>
-#include <stdlib.h>
-
-#include "ml_mputest.h"
-
-#include "mlmath.h"
-#include "mlinclude.h"
-#include "ml.h"
-#include "mlstates.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
-    Globals
-*/
-extern struct mldl_cfg *mputestCfgPtr;
-extern signed char g_z_sign;
-
-/*
-    Prototypes
-*/
-extern inv_error_t inv_factory_calibrate(void *mlsl_handle,
-                                         uint_fast8_t provide_result);
-
-/**
- *  @brief  An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
- *          See inv_factory_calibrate() function for more details.
- *
- *  @pre    inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
- *          data structure.
- *          On Windows, SetupPlatform() is also a madatory pre condition to
- *          ensure the accelerometer is properly configured before running the
- *          test.
- *
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  provide_result
- *              If 1:
- *              perform and analyze the offset, drive frequency, and noise
- *              calculation and compare it against set thresholds. Report
- *              also the final result using a bit-mask like error code as
- *              described in the inv_test_gyro_xxxx() functions.
- *              When 0:
- *              skip the noise and drive frequency calculation  and pass/fail
- *              assessment. It simply calculates the gyro and accel biases.
- *              NOTE: for MPU6050 devices, this parameter is currently
- *              ignored.
- *
- *  @return INV_SUCCESS or first non-zero error code otherwise.
- */
-inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
-                                            unsigned char provide_result)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t firstError = INV_SUCCESS;
-    inv_error_t result;
-    unsigned char initState = inv_get_state();
-
-    if (initState < INV_STATE_DMP_OPENED) {
-        MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
-        return INV_ERROR_SM_IMPROPER_STATE;
-    }
-
-    /* obtain a pointer to the 'struct mldl_cfg' data structure. */
-    mputestCfgPtr = inv_get_dl_config();
-
-    if(initState == INV_STATE_DMP_STARTED) {
-        result = inv_dmp_stop();
-        ERROR_CHECK_FIRST(firstError, result);
-    }
-
-    result = inv_factory_calibrate(mlsl_handle, provide_result);
-    ERROR_CHECK_FIRST(firstError, result);
-
-    if(initState == INV_STATE_DMP_STARTED) {
-        result = inv_dmp_start();
-        ERROR_CHECK_FIRST(firstError, result);
-    }
-
-    return firstError;
-}
-
-/**
- *  @brief  Runs the MPU test at MPL runtime.
- *          If the DMP is operating, stops the DMP temporarely,
- *          runs the MPU Self Test, and re-starts the DMP.
- *
- *  @return INV_SUCCESS or a non-zero error code otherwise.
- */
-inv_error_t inv_self_test_run(void)
-{
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-    return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE);
-#else
-    return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
-#endif
-}
-
-/**
- *  @brief  Runs the MPU test for bias correction only at MPL runtime.
- *          If the DMP is operating, stops the DMP temporarely,
- *          runs the bias calculation routines, and re-starts the DMP.
- *
- *  @return INV_SUCCESS or a non-zero error code otherwise.
- */
-inv_error_t inv_self_test_bias_only(void)
-{
-    return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
-}
-
-/**
- *  @brief  Set the orientation of the acceleroemter Z axis as it will be
- *          expected when running the MPU Self Test.
- *          Specifies the orientation of the accelerometer Z axis : Z axis
- *          pointing upwards or downwards.
- *  @param  z_sign
- *              The sign of the accelerometer Z axis; valid values are +1 and
- *              -1 for +Z and -Z respectively.  Any other value will cause the
- *              setting to be ignored and an error code to be returned.
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign)
-{
-    if (z_sign != +1 && z_sign != -1) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    g_z_sign = z_sign;
-    return INV_SUCCESS;
-}
-
-
-#ifdef __cplusplus
-}
-#endif
-
-/**
- *  @}
- */
-
diff --git a/mlsdk/mllite/ml_mputest.h b/mlsdk/mllite/ml_mputest.h
deleted file mode 100644
index 705d3cc..0000000
--- a/mlsdk/mllite/ml_mputest.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: ml_mputest.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *****************************************************************************/
-
-#ifndef _INV_MPUTEST_H_
-#define _INV_MPUTEST_H_
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-
-/* user APIs */
-inv_error_t inv_self_test_factory_calibrate(
-                void *mlsl_handle, unsigned char provide_result);
-inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign);
-inv_error_t inv_self_test_run(void);
-inv_error_t inv_self_test_bias_only(void);
-
-/* other functions */
-#define inv_set_self_test_parameters inv_set_test_parameters
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _INV_MPUTEST_H_ */
-
diff --git a/mlsdk/mllite/ml_stored_data.c b/mlsdk/mllite/ml_stored_data.c
deleted file mode 100644
index 9107fe2..0000000
--- a/mlsdk/mllite/ml_stored_data.c
+++ /dev/null
@@ -1,1586 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML_STORED_DATA
- *
- * @{
- *      @file     ml_stored_data.c
- *      @brief    functions for reading and writing stored data sets.
- *                Typically, these functions process stored calibration data.
- */
-
-#include "ml_stored_data.h"
-#include "ml.h"
-#include "mlFIFO.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "checksum.h"
-#include "mlsupervisor.h"
-
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-storeload"
-
-/*
-    Typedefs
-*/
-typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short);
-
-/*
-    Globals
-*/
-extern struct inv_obj_t inv_obj;
-
-/*
-    Debugging Definitions
-    set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields
-    being read or stored in human-readable format.
-    When set to 0, the compiler will optimize and remove the printouts
-    from the code.
-*/
-#define LOADCAL_DEBUG    0
-#define STORECAL_DEBUG   0
-
-#define LOADCAL_LOG(...)                        \
-    if(LOADCAL_DEBUG)                           \
-        MPL_LOGI("LOADCAL: " __VA_ARGS__)
-#define STORECAL_LOG(...)                       \
-    if(STORECAL_DEBUG)                          \
-        MPL_LOGI("STORECAL: " __VA_ARGS__)
-
-/**
- *  @brief  Duplicate of the inv_temp_comp_find_bin function in the libmpl
- *          advanced algorithms library. To remove cross-dependency, for now,
- *          we reimplement the same function here.
- *  @param  temp
- *              the temperature (1 count == 1 degree C).
- */
-int FindTempBin(float temp)
-{
-    int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN);
-    if (bin < 0)
-        bin = 0;
-    if (bin > BINS - 1)
-        bin = BINS - 1;
-    return bin;
-}
-
-/**
- * @brief   Returns the length of the <b>MPL internal calibration data</b>.
- *          Should be called before allocating the memory required to store
- *          this data to a file.
- *          This function returns the total size required to store the cal
- *          data including the header (4 bytes) and the checksum (2 bytes).
- *
- *  @pre    Must be in INV_STATE_DMP_OPENED state.
- *          inv_dmp_open() or inv_dmp_stop() must have been called.
- *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- *          been called.
- *
- * @return  the length of the internal calibrated data format.
- */
-unsigned int inv_get_cal_length(void)
-{
-    INVENSENSE_FUNC_START;
-    unsigned int length;
-    length = INV_CAL_HDR_LEN +  // header
-        BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal
-        INV_CAL_ACCEL_LEN +     // accel cal
-        INV_CAL_COMPASS_LEN +   // compass cal
-        INV_CAL_CHK_LEN;        // checksum
-    return length;
-}
-
-/**
- *  @brief  Loads a type 0 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature,
- *          - gyro biases for X, Y, Z axes.
- *          This calibration data would normally be produced by the MPU Self
- *          Test and its size is 18 bytes (header and checksum included).
- *          Calibration format type 0 is currently <b>NOT</b> used and
- *          is substituted by type 5 : inv_load_cal_V5().
- *
- *  @note   This calibration data format is obsoleted and no longer supported
- *          by the rest of the MPL
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    const short expLen = 18;
-    long newGyroData[3] = { 0, 0, 0 };
-    float newTemp;
-    int bin;
-
-    LOADCAL_LOG("Entering inv_load_cal_V0\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen);
-        return INV_ERROR_FILE_READ;
-    }
-
-    newTemp = (float)inv_decode_temperature(
-                (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
-    LOADCAL_LOG("newTemp = %f\n", newTemp);
-
-    newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
-    if (newGyroData[0] > 32767L)
-        newGyroData[0] -= 65536L;
-    LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]);
-    newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
-    if (newGyroData[1] > 32767L)
-        newGyroData[1] -= 65536L;
-    LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
-    newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
-    if (newGyroData[2] > 32767L)
-        newGyroData[2] -= 65536L;
-    LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
-
-    bin = FindTempBin(newTemp);
-    LOADCAL_LOG("bin = %d", bin);
-
-    inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
-    LOADCAL_LOG("temp_data[%d][%d] = %f",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
-        ((float)newGyroData[0]) / 65536.f;
-    LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
-        ((float)newGyroData[0]) / 65536.f;
-    LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
-        ((float)newGyroData[0]) / 65536.f;
-    LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-
-    inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
-    LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
-    if (inv_obj.temp_ptrs[bin] == 0)
-        inv_obj.temp_valid_data[bin] = TRUE;
-    LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                bin, inv_obj.temp_valid_data[bin]);
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V0\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Loads a type 1 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature,
- *          - gyro biases for X, Y, Z axes,
- *          - accel biases for X, Y, Z axes.
- *          This calibration data would normally be produced by the MPU Self
- *          Test and its size is 24 bytes (header and checksum included).
- *          Calibration format type 1 is currently <b>NOT</b> used and
- *          is substituted by type 5 : inv_load_cal_V5().
- *
- *  @note   In order to successfully work, the gyro bias must be stored
- *          expressed in 250 dps full scale (131.072 sensitivity). Other full
- *          scale range will produce unpredictable results in the gyro biases.
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    const short expLen = 24;
-    long newGyroData[3] = {0, 0, 0};
-    long accelBias[3] = {0, 0, 0};
-    float gyroBias[3] = {0, 0, 0};
-    float newTemp = 0;
-    int bin;
-
-    LOADCAL_LOG("Entering inv_load_cal_V1\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen);
-        return INV_ERROR_FILE_READ;
-    }
-
-    newTemp = (float)inv_decode_temperature(
-                (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
-    LOADCAL_LOG("newTemp = %f\n", newTemp);
-
-    newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
-    if (newGyroData[0] > 32767L)
-        newGyroData[0] -= 65536L;
-    LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]);
-    newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
-    if (newGyroData[1] > 32767L)
-        newGyroData[1] -= 65536L;
-    LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]);
-    newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
-    if (newGyroData[2] > 32767L)
-        newGyroData[2] -= 65536L;
-    LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
-
-    bin = FindTempBin(newTemp);
-    LOADCAL_LOG("bin = %d\n", bin);
-
-    gyroBias[0] = ((float)newGyroData[0]) / 131.072f;
-    gyroBias[1] = ((float)newGyroData[1]) / 131.072f;
-    gyroBias[2] = ((float)newGyroData[2]) / 131.072f;
-    LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]);
-    LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]);
-    LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]);
-
-    inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
-    LOADCAL_LOG("temp_data[%d][%d] = %f",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0];
-    LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1];
-    LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-    inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2];
-    LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin],
-                inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-
-    inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
-    LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
-    if (inv_obj.temp_ptrs[bin] == 0)
-        inv_obj.temp_valid_data[bin] = TRUE;
-    LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                bin, inv_obj.temp_valid_data[bin]);
-
-    /* load accel biases and apply immediately */
-    accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]);
-    if (accelBias[0] > 32767)
-        accelBias[0] -= 65536L;
-    accelBias[0] = (long)((long long)accelBias[0] * 65536L *
-                          inv_obj.accel_sens / 1073741824L);
-    LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]);
-    accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]);
-    if (accelBias[1] > 32767)
-        accelBias[1] -= 65536L;
-    accelBias[1] = (long)((long long)accelBias[1] * 65536L *
-                          inv_obj.accel_sens / 1073741824L);
-    LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]);
-    accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]);
-    if (accelBias[2] > 32767)
-        accelBias[2] -= 65536L;
-    accelBias[2] = (long)((long long)accelBias[2] * 65536L *
-                          inv_obj.accel_sens / 1073741824L);
-    LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]);
-    if (inv_set_array(INV_ACCEL_BIAS, accelBias)) {
-        LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias));
-        return inv_set_array(INV_ACCEL_BIAS, accelBias);
-    }
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V1\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Loads a type 2 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature compensation : temperature data points,
- *          - temperature compensation : gyro biases data points for X, Y,
- *              and Z axes.
- *          - accel biases for X, Y, Z axes.
- *          This calibration data is produced internally by the MPL and its
- *          size is 2222 bytes (header and checksum included).
- *          Calibration format type 2 is currently <b>NOT</b> used and
- *          is substituted by type 4 : inv_load_cal_V4().
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    const short expLen = 2222;
-    long accel_bias[3];
-    int ptr = INV_CAL_HDR_LEN;
-
-    int i, j;
-    long long tmp;
-
-    LOADCAL_LOG("Entering inv_load_cal_V2\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n",
-                 expLen, len);
-        return INV_ERROR_FILE_READ;
-    }
-
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_ptrs[i] = 0;
-        inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
-    }
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_valid_data[i] = 0;
-        inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                    i, inv_obj.temp_valid_data[i]);
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.temp_data[i][j]);
-        }
-
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.x_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.y_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.z_gyro_temp_data[i][j]);
-        }
-    }
-
-    /* read the accel biases */
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        accel_bias[i] = (int32_t) t;
-        LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]);
-    }
-
-    if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) {
-        LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias));
-        return inv_set_array(INV_ACCEL_BIAS, accel_bias);
-    }
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V2\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Loads a type 3 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature compensation : temperature data points,
- *          - temperature compensation : gyro biases data points for X, Y,
- *              and Z axes.
- *          - accel biases for X, Y, Z axes.
- *          - compass biases for X, Y, Z axes and bias tracking algorithm
- *              mock-up.
- *          This calibration data is produced internally by the MPL and its
- *          size is 2429 bytes (header and checksum included).
- *          Calibration format type 3 is currently <b>NOT</b> used and
- *          is substituted by type 4 : inv_load_cal_V4().
-
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    union doubleToLongLong {
-        double db;
-        unsigned long long ll;
-    } dToLL;
-
-    const short expLen = 2429;
-    long bias[3];
-    int i, j;
-    int ptr = INV_CAL_HDR_LEN;
-    long long tmp;
-
-    LOADCAL_LOG("Entering inv_load_cal_V3\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n",
-                 expLen, len);
-        return INV_ERROR_FILE_READ;
-    }
-
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_ptrs[i] = 0;
-        inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
-    }
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_valid_data[i] = 0;
-        inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                    i, inv_obj.temp_valid_data[i]);
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.temp_data[i][j]);
-        }
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.x_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.y_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.z_gyro_temp_data[i][j]);
-        }
-    }
-
-    /* read the accel biases */
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        bias[i] = (int32_t) t;
-        LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]);
-    }
-    if (inv_set_array(INV_ACCEL_BIAS, bias)) {
-        LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias));
-        return inv_set_array(INV_ACCEL_BIAS, bias);
-    }
-
-    /* read the compass biases */
-    inv_obj.got_compass_bias = (int)calData[ptr++];
-    inv_obj.got_init_compass_bias = (int)calData[ptr++];
-    inv_obj.compass_state = (int)calData[ptr++];
-
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_bias_error[i] = (int32_t) t;
-        LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i,
-                    inv_obj.compass_bias_error[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.init_compass_bias[i] = (int32_t) t;
-        LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i,
-                    inv_obj.init_compass_bias[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_bias[i] = (int32_t) t;
-        LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
-    }
-    for (i = 0; i < 18; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_peaks[i] = (int32_t) t;
-        LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        dToLL.ll = 0;
-        dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_bias_v[i] = dToLL.db;
-        LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]);
-    }
-    for (i = 0; i < 9; i++) {
-        dToLL.ll = 0;
-        dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_bias_ptr[i] = dToLL.db;
-        LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
-                    inv_obj.compass_bias_ptr[i]);
-    }
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V3\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Loads a type 4 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature compensation : temperature data points,
- *          - temperature compensation : gyro biases data points for X, Y,
- *              and Z axes.
- *          - accel biases for X, Y, Z axes.
- *          - compass biases for X, Y, Z axes, compass scale, and bias
- *              tracking algorithm  mock-up.
- *          This calibration data is produced internally by the MPL and its
- *          size is 2777 bytes (header and checksum included).
- *          Calibration format type 4 is currently used and
- *          substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()).
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    union doubleToLongLong {
-        double db;
-        unsigned long long ll;
-    } dToLL;
-
-    const unsigned int expLen = 2782;
-    long bias[3];
-    int ptr = INV_CAL_HDR_LEN;
-    int i, j;
-    long long tmp;
-    inv_error_t result;
-
-    LOADCAL_LOG("Entering inv_load_cal_V4\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n",
-                 expLen, len);
-        return INV_ERROR_FILE_READ;
-    }
-
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_ptrs[i] = 0;
-        inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_ptrs[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
-    }
-    for (i = 0; i < BINS; i++) {
-        inv_obj.temp_valid_data[i] = 0;
-        inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
-        inv_obj.temp_valid_data[i] += (int)calData[ptr++];
-        LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                    i, inv_obj.temp_valid_data[i]);
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.temp_data[i][j]);
-        }
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.x_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.y_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = 0;
-            tmp += 16777216LL * (long long)calData[ptr++];
-            tmp += 65536LL * (long long)calData[ptr++];
-            tmp += 256LL * (long long)calData[ptr++];
-            tmp += (long long)calData[ptr++];
-            if (tmp > 2147483648LL) {
-                tmp -= 4294967296LL;
-            }
-            inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
-            LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                        i, j, inv_obj.z_gyro_temp_data[i][j]);
-        }
-    }
-
-    /* read the accel biases */
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        bias[i] = (int32_t) t;
-        LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]);
-    }
-    if (inv_set_array(INV_ACCEL_BIAS, bias)) {
-        LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias));
-        return inv_set_array(INV_ACCEL_BIAS, bias);
-    }
-
-    /* read the compass biases */
-    inv_reset_compass_calibration();
-
-    inv_obj.got_compass_bias = (int)calData[ptr++];
-    LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias);
-    inv_obj.got_init_compass_bias = (int)calData[ptr++];
-    LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias);
-    inv_obj.compass_state = (int)calData[ptr++];
-    LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state);
-
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_bias_error[i] = (int32_t) t;
-        LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i,
-                    inv_obj.compass_bias_error[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.init_compass_bias[i] = (int32_t) t;
-        LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i,
-                    inv_obj.init_compass_bias[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_bias[i] = (int32_t) t;
-        LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
-    }
-    for (i = 0; i < 18; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_peaks[i] = (int32_t) t;
-        LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_bias_v[i] = dToLL.db;
-        LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]);
-    }
-    for (i = 0; i < 9; i++) {
-        dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_bias_ptr[i] = dToLL.db;
-        LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
-                    inv_obj.compass_bias_ptr[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = 0;
-        t += 16777216UL * ((uint32_t) calData[ptr++]);
-        t += 65536UL * ((uint32_t) calData[ptr++]);
-        t += 256u * ((uint32_t) calData[ptr++]);
-        t += (uint32_t) calData[ptr++];
-        inv_obj.compass_scale[i] = (int32_t) t;
-        LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t);
-    }
-    for (i = 0; i < 6; i++) {
-        dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_prev_xty[i] = dToLL.db;
-        LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db);
-    }
-    for (i = 0; i < 36; i++) {
-        dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
-        dToLL.ll += (unsigned long long)calData[ptr++];
-
-        inv_obj.compass_prev_m[i] = dToLL.db;
-        LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db);
-    }
-
-    /* Load the compass offset flag and values */
-    inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++];
-    inv_obj.compass_offsets[0] = calData[ptr++];
-    inv_obj.compass_offsets[1] = calData[ptr++];
-    inv_obj.compass_offsets[2] = calData[ptr++];
-
-    inv_obj.compass_accuracy = calData[ptr++];
-    /* push the compass offset values to the device */
-    result = inv_set_compass_offset();
-
-    if (result == INV_SUCCESS) {
-        if (inv_compass_check_range() != INV_SUCCESS) {
-            MPL_LOGI("range check fail");
-            inv_reset_compass_calibration();
-            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
-            inv_set_compass_offset();
-        }
-    }
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V4\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Loads a type 5 set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *          This calibrations data format stores values for (in order of
- *          appearance) :
- *          - temperature,
- *          - gyro biases for X, Y, Z axes,
- *          - accel biases for X, Y, Z axes.
- *          This calibration data would normally be produced by the MPU Self
- *          Test and its size is 36 bytes (header and checksum included).
- *          Calibration format type 5 is produced by the MPU Self Test and
- *          substitutes the type 1 : inv_load_cal_V1().
- *
- *  @pre    inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- *  @param  calData
- *              A pointer to an array of bytes to be parsed.
- *  @param  len
- *              the length of the calibration
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len)
-{
-    INVENSENSE_FUNC_START;
-    const short expLen = 36;
-    long accelBias[3] = { 0, 0, 0 };
-    float gyroBias[3] = { 0, 0, 0 };
-
-    int ptr = INV_CAL_HDR_LEN;
-    unsigned short temp;
-    float newTemp;
-    int bin;
-    int i;
-
-    LOADCAL_LOG("Entering inv_load_cal_V5\n");
-
-    if (len != expLen) {
-        MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n",
-                 expLen, len);
-        return INV_ERROR_FILE_READ;
-    }
-
-    /* load the temperature */
-    temp = (unsigned short)calData[ptr++] * (1L << 8);
-    temp += calData[ptr++];
-    newTemp = (float)inv_decode_temperature(temp) / 65536.f;
-    LOADCAL_LOG("newTemp = %f\n", newTemp);
-
-    /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */
-    for (i = 0; i < 3; i++) {
-        int32_t tmp = 0;
-        tmp += (int32_t) calData[ptr++] * (1L << 24);
-        tmp += (int32_t) calData[ptr++] * (1L << 16);
-        tmp += (int32_t) calData[ptr++] * (1L << 8);
-        tmp += (int32_t) calData[ptr++];
-        gyroBias[i] = (float)tmp / 65536.0f;
-        LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]);
-    }
-    /* find the temperature bin */
-    bin = FindTempBin(newTemp);
-
-    /* populate the temp comp data structure */
-    inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
-    LOADCAL_LOG("temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin], newTemp);
-
-    inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0];
-    LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin], gyroBias[0]);
-    inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1];
-    LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin], gyroBias[1]);
-    inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2];
-    LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                bin, inv_obj.temp_ptrs[bin], gyroBias[2]);
-    inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
-    LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
-    if (inv_obj.temp_ptrs[bin] == 0)
-        inv_obj.temp_valid_data[bin] = TRUE;
-    LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
-                bin, inv_obj.temp_valid_data[bin]);
-
-    /* load accel biases (represented in 2 ^ 16 == 1 gee)
-       and apply immediately */
-    for (i = 0; i < 3; i++) {
-        int32_t tmp = 0;
-        tmp += (int32_t) calData[ptr++] * (1L << 24);
-        tmp += (int32_t) calData[ptr++] * (1L << 16);
-        tmp += (int32_t) calData[ptr++] * (1L << 8);
-        tmp += (int32_t) calData[ptr++];
-        accelBias[i] = (long)tmp;
-        LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]);
-    }
-    if (inv_set_array(INV_ACCEL_BIAS, accelBias)) {
-        LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias));
-        return inv_set_array(INV_ACCEL_BIAS, accelBias);
-    }
-
-    inv_obj.got_no_motion_bias = TRUE;
-    LOADCAL_LOG("got_no_motion_bias = 1\n");
-    inv_obj.cal_loaded_flag = TRUE;
-    LOADCAL_LOG("cal_loaded_flag = 1\n");
-
-    LOADCAL_LOG("Exiting inv_load_cal_V5\n");
-    return INV_SUCCESS;
-}
-
-/**
- * @brief   Loads a set of calibration data.
- *          It parses a binary data set containing calibration data.
- *          The binary data set is intended to be loaded from a file.
- *
- * @pre     inv_dmp_open()
- *          @ifnot MPL_MF
- *              or inv_open_low_power_pedometer()
- *              or inv_eis_open_dmp()
- *          @endif
- *          must have been called.
- *
- * @param   calData
- *              A pointer to an array of bytes to be parsed.
- *
- * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal(unsigned char *calData)
-{
-    INVENSENSE_FUNC_START;
-    int calType = 0;
-    int len = 0;
-    int ptr;
-    uint32_t chk = 0;
-    uint32_t cmp_chk = 0;
-
-    tMLLoadFunc loaders[] = {
-        inv_load_cal_V0,
-        inv_load_cal_V1,
-        inv_load_cal_V2,
-        inv_load_cal_V3,
-        inv_load_cal_V4,
-        inv_load_cal_V5
-    };
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    /* read the header (type and len)
-       len is the total record length including header and checksum */
-    len = 0;
-    len += 16777216L * ((int)calData[0]);
-    len += 65536L * ((int)calData[1]);
-    len += 256 * ((int)calData[2]);
-    len += (int)calData[3];
-
-    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
-    if (calType > 5) {
-        MPL_LOGE("Unsupported calibration file format %d. "
-                 "Valid types 0..5\n", calType);
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    /* check the checksum */
-    chk = 0;
-    ptr = len - INV_CAL_CHK_LEN;
-
-    chk += 16777216L * ((uint32_t) calData[ptr++]);
-    chk += 65536L * ((uint32_t) calData[ptr++]);
-    chk += 256 * ((uint32_t) calData[ptr++]);
-    chk += (uint32_t) calData[ptr++];
-
-    cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN,
-                           len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN));
-
-    if (chk != cmp_chk) {
-        return INV_ERROR_CALIBRATION_CHECKSUM;
-    }
-
-    /* call the proper method to read in the data */
-    return loaders[calType] (calData, len);
-}
-
-/**
- *  @brief  Stores a set of calibration data.
- *          It generates a binary data set containing calibration data.
- *          The binary data set is intended to be stored into a file.
- *
- *  @pre    inv_dmp_open()
- *
- *  @param  calData
- *              A pointer to an array of bytes to be stored.
- *  @param  length
- *              The amount of bytes available in the array.
- *
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_store_cal(unsigned char *calData, int length)
-{
-    INVENSENSE_FUNC_START;
-    int ptr = 0;
-    int i = 0;
-    int j = 0;
-    long long tmp;
-    uint32_t chk;
-    long bias[3];
-    //unsigned char state;
-    union doubleToLongLong {
-        double db;
-        unsigned long long ll;
-    } dToLL;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    STORECAL_LOG("Entering inv_store_cal\n");
-
-    // length
-    calData[0] = (unsigned char)((length >> 24) & 0xff);
-    calData[1] = (unsigned char)((length >> 16) & 0xff);
-    calData[2] = (unsigned char)((length >> 8) & 0xff);
-    calData[3] = (unsigned char)(length & 0xff);
-    STORECAL_LOG("calLen = %d\n", length);
-
-    // calibration data format type
-    calData[4] = 0;
-    calData[5] = 4;
-    STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]);
-
-    // data
-    ptr = 6;
-    for (i = 0; i < BINS; i++) {
-        tmp = (int)inv_obj.temp_ptrs[i];
-        calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(tmp & 0xff);
-        STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp);
-    }
-
-    for (i = 0; i < BINS; i++) {
-        tmp = (int)inv_obj.temp_valid_data[i];
-        calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(tmp & 0xff);
-        STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp);
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f);
-            if (tmp < 0) {
-                tmp += 4294967296LL;
-            }
-            calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-            calData[ptr++] = (unsigned char)(tmp & 0xff);
-            STORECAL_LOG("temp_data[%d][%d] = %f\n",
-                         i, j, inv_obj.temp_data[i][j]);
-        }
-    }
-
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f);
-            if (tmp < 0) {
-                tmp += 4294967296LL;
-            }
-            calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-            calData[ptr++] = (unsigned char)(tmp & 0xff);
-            STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
-                         i, j, inv_obj.x_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f);
-            if (tmp < 0) {
-                tmp += 4294967296LL;
-            }
-            calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-            calData[ptr++] = (unsigned char)(tmp & 0xff);
-            STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
-                         i, j, inv_obj.y_gyro_temp_data[i][j]);
-        }
-    }
-    for (i = 0; i < BINS; i++) {
-        for (j = 0; j < PTS_PER_BIN; j++) {
-            tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f);
-            if (tmp < 0) {
-                tmp += 4294967296LL;
-            }
-            calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
-            calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
-            calData[ptr++] = (unsigned char)(tmp & 0xff);
-            STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
-                         i, j, inv_obj.z_gyro_temp_data[i][j]);
-        }
-    }
-
-    inv_get_array(INV_ACCEL_BIAS, bias);
-
-    /* write the accel biases */
-    for (i = 0; i < 3; i++) {
-        uint32_t t = (uint32_t) bias[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]);
-    }
-
-    /* write the compass calibration state */
-    calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias);
-    STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias);
-    calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias);
-    STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias);
-    if (inv_obj.compass_state == SF_UNCALIBRATED) {
-        calData[ptr++] = SF_UNCALIBRATED;
-    } else {
-        calData[ptr++] = SF_STARTUP_SETTLE;
-    }
-    STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state);
-
-    for (i = 0; i < 3; i++) {
-        uint32_t t = (uint32_t) inv_obj.compass_bias_error[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("compass_bias_error[%d] = %ld\n",
-                     i, inv_obj.compass_bias_error[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = (uint32_t) inv_obj.init_compass_bias[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("init_compass_bias[%d] = %ld\n", i,
-                     inv_obj.init_compass_bias[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = (uint32_t) inv_obj.compass_bias[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
-    }
-    for (i = 0; i < 18; i++) {
-        uint32_t t = (uint32_t) inv_obj.compass_peaks[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        dToLL.db = inv_obj.compass_bias_v[i];
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
-        STORECAL_LOG("compass_bias_v[%d] = %lf\n", i,
-                     inv_obj.compass_bias_v[i]);
-    }
-    for (i = 0; i < 9; i++) {
-        dToLL.db = inv_obj.compass_bias_ptr[i];
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
-        STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
-                     inv_obj.compass_bias_ptr[i]);
-    }
-    for (i = 0; i < 3; i++) {
-        uint32_t t = (uint32_t) inv_obj.compass_scale[i];
-        calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(t & 0xff);
-        STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]);
-    }
-    for (i = 0; i < 6; i++) {
-        dToLL.db = inv_obj.compass_prev_xty[i];
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
-        STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i,
-                     inv_obj.compass_prev_xty[i]);
-    }
-    for (i = 0; i < 36; i++) {
-        dToLL.db = inv_obj.compass_prev_m[i];
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
-        calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
-        calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
-        STORECAL_LOG("compass_prev_m[%d] = %lf\n", i,
-                     inv_obj.compass_prev_m[i]);
-    }
-
-    /* store the compass offsets and validity flag */
-    calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID];
-    calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0];
-    calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1];
-    calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2];
-
-    /* store the compass accuracy */
-    calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy);
-
-    /* add a checksum */
-    chk = inv_checksum(calData + INV_CAL_HDR_LEN,
-                       length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN));
-    calData[ptr++] = (unsigned char)((chk >> 24) & 0xff);
-    calData[ptr++] = (unsigned char)((chk >> 16) & 0xff);
-    calData[ptr++] = (unsigned char)((chk >> 8) & 0xff);
-    calData[ptr++] = (unsigned char)(chk & 0xff);
-
-    STORECAL_LOG("Exiting inv_store_cal\n");
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Load a calibration file.
- *
- *  @pre    Must be in INV_STATE_DMP_OPENED state.
- *          inv_dmp_open() or inv_dmp_stop() must have been called.
- *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- *          been called.
- *
- *  @return 0 or error code.
- */
-inv_error_t inv_load_calibration(void)
-{
-    unsigned char *calData;
-    inv_error_t result;
-    unsigned int length;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    result = inv_serial_get_cal_length(&length);
-    if (result == INV_ERROR_FILE_OPEN) {
-        MPL_LOGI("Calibration data not loaded\n");
-        return INV_SUCCESS;
-    }
-    if (result || length <= 0) {
-        MPL_LOGE("Could not get file calibration length - "
-                 "error %d - aborting\n", result);
-        return result;
-    }
-    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_serial_read_cal(calData, length);
-    if (result) {
-        MPL_LOGE("Could not read the calibration data from file - "
-                 "error %d - aborting\n", result);
-        goto free_mem_n_exit;
-
-    }
-    result = inv_load_cal(calData);
-    if (result) {
-        MPL_LOGE("Could not load the calibration data - "
-                 "error %d - aborting\n", result);
-        goto free_mem_n_exit;
-
-    }
-
-
-
-free_mem_n_exit:    
-    inv_free(calData);
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Store runtime calibration data to a file
- *
- *  @pre    Must be in INV_STATE_DMP_OPENED state.
- *          inv_dmp_open() or inv_dmp_stop() must have been called.
- *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- *          been called.
- *
- *  @return 0 or error code.
- */
-inv_error_t inv_store_calibration(void)
-{
-    unsigned char *calData;
-    inv_error_t result;
-    unsigned int length;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    length = inv_get_cal_length();
-    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_store_cal(calData, length);
-    if (result) {
-        MPL_LOGE("Could not store calibrated data on file - "
-                 "error %d - aborting\n", result);
-        goto free_mem_n_exit;
-
-    }
-    result = inv_serial_write_cal(calData, length);
-    if (result) {
-        MPL_LOGE("Could not write calibration data - " "error %d\n", result);
-        goto free_mem_n_exit;
-
-    }
-
-
-
-free_mem_n_exit:
-    inv_free(calData);
-    return INV_SUCCESS;
-}
-
-/**
- *  @}
- */
diff --git a/mlsdk/mllite/ml_stored_data.h b/mlsdk/mllite/ml_stored_data.h
deleted file mode 100644
index 74c2b7c..0000000
--- a/mlsdk/mllite/ml_stored_data.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef INV_STORED_DATA_H
-#define INV_STORED_DATA_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
-    Includes.
-*/
-
-#include "mltypes.h"
-
-/*
-    Defines
-*/
-#define INV_CAL_ACCEL_LEN    (12)
-#define INV_CAL_COMPASS_LEN  (555 + 5)
-#define INV_CAL_HDR_LEN      (6)
-#define INV_CAL_CHK_LEN      (4)
-
-/*
-    APIs
-*/
-    inv_error_t inv_load_calibration(void);
-    inv_error_t inv_store_calibration(void);
-
-/*
-    Other prototypes
-*/
-    inv_error_t inv_load_cal(unsigned char *calData);
-    inv_error_t inv_store_cal(unsigned char *calData, int length);
-    unsigned int inv_get_cal_length(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          /* INV_STORED_DATA_H */
diff --git a/mlsdk/mllite/mlarray.c b/mlsdk/mllite/mlarray.c
deleted file mode 100644
index 6ae85e0..0000000
--- a/mlsdk/mllite/mlarray.c
+++ /dev/null
@@ -1,2524 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- *
- * $Id: mlarray.c 5085 2011-04-08 22:25:14Z phickey $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup ML
- *  @{
- *      @file   mlarray.c
- *      @brief  APIs to read different data sets from FIFO.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-#include "ml.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mlMathFunc.h"
-#include "mlmath.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlsupervisor.h"
-#include "mldl.h"
-#include "dmpKey.h"
-#include "compass.h"
-
-/**
- *  @brief  inv_get_gyro is used to get the most recent gyroscope measurement.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled at 1 dps = 2^16 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-
- /* inv_get_gyro implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_accel is used to get the most recent
- *          accelerometer measurement.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled in units of g (gravity),
- *          where 1 g = 2^16 LSBs.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_accel implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_temperature is used to get the most recent
- *          temperature measurement.
- *          The argument array should only have one element.
- *          The value is in units of deg C (degrees Celsius), where
- *          2^16 LSBs = 1 deg C.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to the data to be passed back to the user.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_temperature implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_rot_mat is used to get the rotation matrix
- *          representation of the current sensor fusion solution.
- *          The array format will be R11, R12, R13, R21, R22, R23, R31, R32,
- *          R33, representing the matrix:
- *          <center>R11 R12 R13</center>
- *          <center>R21 R22 R23</center>
- *          <center>R31 R32 R33</center>
- *          Values are scaled, where 1.0 = 2^30 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_rot_mat(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    long qdata[4];
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    inv_get_quaternion(qdata);
-    inv_quaternion_to_rotation(qdata, data);
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_quaternion is used to get the quaternion representation
- *          of the current sensor fusion solution.
- *          The values are scaled where 1.0 = 2^30 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 4 cells long </b>.
- *
- *  @return INV_SUCCESS if the command is successful; an ML error code otherwise.
- */
- /* inv_get_quaternion implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_linear_accel is used to get an estimate of linear
- *          acceleration, based on the most recent accelerometer measurement
- *          and sensor fusion solution.
- *          The values are scaled where 1 g (gravity) = 2^16 LSBs.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_linear_accel implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_linear_accel_in_world is used to get an estimate of
- *          linear acceleration, in the world frame,  based on the most
- *          recent accelerometer measurement and sensor fusion solution.
- *          The values are scaled where 1 g (gravity) = 2^16 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_linear_accel_in_world implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_gravity is used to get an estimate of the body frame
- *          gravity vector, based on the most recent sensor fusion solution.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_gravity implemented in mlFIFO.c */
-
-/**
- *  @internal
- *  @brief  inv_get_angular_velocity is used to get an estimate of the body
- *          frame angular velocity, which is computed from the current and
- *          previous sensor fusion solutions.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_angular_velocity(long *data)
-{
-
-    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    /* not implemented. old (invalid) implementation:
-       if ( inv_get_state() < INV_STATE_DMP_OPENED )
-       return INV_ERROR_SM_IMPROPER_STATE;
-
-       if (NULL == data) {
-       return INV_ERROR_INVALID_PARAMETER;
-       }
-       data[0] = inv_obj.ang_v_body[0];
-       data[1] = inv_obj.ang_v_body[1];
-       data[2] = inv_obj.ang_v_body[2];
-
-       return result;
-     */
-}
-
-/**
- *  @brief  inv_get_euler_angles is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          Euler angles may follow various conventions. This function is equivelant
- *          to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more
- *          information.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles(long *data)
-{
-    return inv_get_euler_angles_x(data);
-}
-
-/**
- *  @brief  inv_get_euler_angles_x is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          Euler angles are returned according to the X convention.
- *          This is typically the convention used for mobile devices where the X
- *          axis is the width of the screen, Y axis is the height, and Z the
- *          depth. In this case roll is defined as the rotation around the X
- *          axis of the device.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>Y axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
- *          </TABLE>
- *
- *          Values are scaled where 1.0 = 2^16.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles_x(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[6];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (long)((float)
-               (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) *
-               65536L);
-    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
-    data[2] =
-        (long)((float)
-               (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) *
-               65536L);
-    return result;
-}
-
-/**
- *  @brief  inv_get_euler_angles_y is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          Euler angles are returned according to the Y convention.
- *          This convention is typically used in augmented reality applications,
- *          where roll is defined as the rotation around the axis along the
- *          height of the screen of a mobile device, namely the Y axis.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Y axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
- *          </TABLE>
- *
- *          Values are scaled where 1.0 = 2^16.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles_y(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[7];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (long)((float)
-               (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) *
-               65536L);
-    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
-    data[2] =
-        (long)((float)
-               (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) *
-               65536L);
-    return result;
-}
-
-/**  @brief  inv_get_euler_angles_z is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          This convention is mostly used in application involving the use
- *          of a camera, typically placed on the back of a mobile device, that
- *          is along the Z axis.  In this convention roll is defined as the
- *          rotation around the Z axis.
- *          Euler angles are returned according to the Y convention.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Z axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Y axis                </TD></TR>
- *          </TABLE>
- *
- *          Values are scaled where 1.0 = 2^16.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-
-inv_error_t inv_get_euler_angles_z(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[8];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (long)((float)
-               (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) *
-               65536L);
-    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
-    data[2] =
-        (long)((float)
-               (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) *
-               65536L);
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_temp_slope is used to get is used to get the temperature
- *          compensation algorithm's estimate of the gyroscope bias temperature
- *          coefficient.
- *          The argument array elements are ordered X,Y,Z.
- *          Values are in units of dps per deg C (degrees per second per degree
- *          Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs.
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_temp_slope(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
-        data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f);
-        data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f);
-        data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f);
-    } else {
-        data[0] = inv_obj.temp_slope[0];
-        data[1] = inv_obj.temp_slope[1];
-        data[2] = inv_obj.temp_slope[2];
-    }
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_bias is used to get the gyroscope biases.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled such that 1 dps = 2^16 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_bias(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    data[0] = inv_obj.gyro_bias[0];
-    data[1] = inv_obj.gyro_bias[1];
-    data[2] = inv_obj.gyro_bias[2];
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_accel_bias is used to get the accelerometer baises.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled such that 1 g (gravity) = 2^16 LSBs.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_accel_bias(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    data[0] = inv_obj.accel_bias[0];
-    data[1] = inv_obj.accel_bias[1];
-    data[2] = inv_obj.accel_bias[2];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_bias is used to get Magnetometer Bias
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_bias(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    data[0] =
-        inv_obj.compass_bias[0] +
-        (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens /
-               16384);
-    data[1] =
-        inv_obj.compass_bias[1] +
-        (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens /
-               16384);
-    data[2] =
-        inv_obj.compass_bias[2] +
-        (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens /
-               16384);
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data.
- *          The argument array elements are ordered gyroscope X,Y, and Z,
- *          accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
- *          \if UMPL The magnetometer elements are not populated in UMPL. \endif
- *          The gyroscope and accelerometer data is not scaled or offset, it is
- *          copied directly from the sensor registers.
- *          In the case of accelerometers with 8-bit output resolution, the data
- *          is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
- *          full scale range
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_raw_data is used to get Raw magnetometer data.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_raw_data(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.compass_sensor_data[0];
-    data[1] = inv_obj.compass_sensor_data[1];
-    data[2] = inv_obj.compass_sensor_data[2];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_magnetometer is used to get magnetometer data.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_magnetometer(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0];
-    data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1];
-    data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_pressure is used to get Pressure data.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to data to be passed back to the user.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_pressure(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.pressure;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_heading is used to get heading from Rotation Matrix.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to data to be passed back to the user.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-
-inv_error_t inv_get_heading(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    result = inv_get_rot_mat_float(rotMatrix);
-    if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
-        tmp =
-            (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
-                    90.0f);
-    } else {
-        tmp =
-            (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
-                    90.0f);
-    }
-    if (tmp < 0) {
-        tmp += 360.0f;
-    }
-    data[0] = (long)((360 - tmp) * 65536.0f);
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_cal_matrix is used to get the gyroscope
- *          calibration matrix. The gyroscope calibration matrix defines the relationship
- *          between the gyroscope sensor axes and the sensor fusion solution axes.
- *          Calibration matrix data members will have a value of 1, 0, or -1.
- *          The matrix has members
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_cal_matrix(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.gyro_cal[0];
-    data[1] = inv_obj.gyro_cal[1];
-    data[2] = inv_obj.gyro_cal[2];
-    data[3] = inv_obj.gyro_cal[3];
-    data[4] = inv_obj.gyro_cal[4];
-    data[5] = inv_obj.gyro_cal[5];
-    data[6] = inv_obj.gyro_cal[6];
-    data[7] = inv_obj.gyro_cal[7];
-    data[8] = inv_obj.gyro_cal[8];
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_accel_cal_matrix is used to get the accelerometer
- *          calibration matrix.
- *          Calibration matrix data members will have a value of 1, 0, or -1.
- *          The matrix has members
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_accel_cal_matrix(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.accel_cal[0];
-    data[1] = inv_obj.accel_cal[1];
-    data[2] = inv_obj.accel_cal[2];
-    data[3] = inv_obj.accel_cal[3];
-    data[4] = inv_obj.accel_cal[4];
-    data[5] = inv_obj.accel_cal[5];
-    data[6] = inv_obj.accel_cal[6];
-    data[7] = inv_obj.accel_cal[7];
-    data[8] = inv_obj.accel_cal[8];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_cal_matrix is used to get magnetometer calibration matrix.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_cal_matrix(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.compass_cal[0];
-    data[1] = inv_obj.compass_cal[1];
-    data[2] = inv_obj.compass_cal[2];
-    data[3] = inv_obj.compass_cal[3];
-    data[4] = inv_obj.compass_cal[4];
-    data[5] = inv_obj.compass_cal[5];
-    data[6] = inv_obj.compass_cal[6];
-    data[7] = inv_obj.compass_cal[7];
-    data[8] = inv_obj.compass_cal[8];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_bias_error is used to get magnetometer Bias error.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_bias_error(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    if (inv_obj.large_field == 0) {
-        data[0] = inv_obj.compass_bias_error[0];
-        data[1] = inv_obj.compass_bias_error[1];
-        data[2] = inv_obj.compass_bias_error[2];
-    } else {
-        data[0] = P_INIT;
-        data[1] = P_INIT;
-        data[2] = P_INIT;
-    }
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_scale is used to get magnetometer scale.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_scale(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.compass_scale[0];
-    data[1] = inv_obj.compass_scale[1];
-    data[2] = inv_obj.compass_scale[2];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_local_field is used to get local magnetic field data.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_local_field(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = inv_obj.local_field[0];
-    data[1] = inv_obj.local_field[1];
-    data[2] = inv_obj.local_field[2];
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_relative_quaternion is used to get relative quaternion.
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 4 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- *  @endcond
- */
-/* inv_get_relative_quaternion implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_gyro_float is used to get the most recent gyroscope measurement.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of dps (degrees per second).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[3];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    result = inv_get_gyro(ldata);
-    data[0] = (float)ldata[0] / 65536.0f;
-    data[1] = (float)ldata[1] / 65536.0f;
-    data[2] = (float)ldata[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  inv_get_angular_velocity_float is used to get an array of three data points representing the angular
- *          velocity as derived from <b>both</b> gyroscopes and accelerometers.
- *          This requires that ML_SENSOR_FUSION be enabled, to fuse data from
- *          the gyroscope and accelerometer device, appropriately scaled and
- *          oriented according to the respective mounting matrices.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_angular_velocity_float(float *data)
-{
-    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    /* not implemented. old (invalid) implementation:
-       return inv_get_gyro_float(data);
-     */
-}
-
-/**
- *  @brief  inv_get_accel_float is used to get the most recent accelerometer measurement.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of g (gravity).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
- /* inv_get_accel_float implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_temperature_float is used to get the most recent
- *          temperature measurement.
- *          The argument array should only have one element.
- *          The value is in units of deg C (degrees Celsius).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to data to be passed back to the user.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_temperature_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[1];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_temperature(ldata);
-    data[0] = (float)ldata[0] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_rot_mat_float is used to get an array of nine data points representing the rotation
- *          matrix generated from all available sensors.
- *          The array format will be R11, R12, R13, R21, R22, R23, R31, R32,
- *          R33, representing the matrix:
- *          <center>R11 R12 R13</center>
- *          <center>R21 R22 R23</center>
- *          <center>R31 R32 R33</center>
- *          <b>Please refer to the "9-Axis Sensor Fusion Application Note" document,
- *          section 7 "Sensor Fusion Output", for details regarding rotation
- *          matrix output</b>.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long at least</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_rot_mat_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    {
-        long qdata[4], rdata[9];
-        inv_get_quaternion(qdata);
-        inv_quaternion_to_rotation(qdata, rdata);
-        data[0] = (float)rdata[0] / 1073741824.0f;
-        data[1] = (float)rdata[1] / 1073741824.0f;
-        data[2] = (float)rdata[2] / 1073741824.0f;
-        data[3] = (float)rdata[3] / 1073741824.0f;
-        data[4] = (float)rdata[4] / 1073741824.0f;
-        data[5] = (float)rdata[5] / 1073741824.0f;
-        data[6] = (float)rdata[6] / 1073741824.0f;
-        data[7] = (float)rdata[7] / 1073741824.0f;
-        data[8] = (float)rdata[8] / 1073741824.0f;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_quaternion_float is used to get the quaternion representation
- *          of the current sensor fusion solution.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 4 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an ML error code otherwise.
- */
- /* inv_get_quaternion_float implemented in mlFIFO.c */
-
-/**
- *  @brief  inv_get_linear_accel_float is used to get an estimate of linear
- *          acceleration, based on the most recent accelerometer measurement
- *          and sensor fusion solution.
- *          The values are in units of g (gravity).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_linear_accel_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[3];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_linear_accel(ldata);
-    data[0] = (float)ldata[0] / 65536.0f;
-    data[1] = (float)ldata[1] / 65536.0f;
-    data[2] = (float)ldata[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_linear_accel_in_world_float is used to get an estimate of
- *          linear acceleration, in the world frame,  based on the most
- *          recent accelerometer measurement and sensor fusion solution.
- *          The values are in units of g (gravity).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_linear_accel_in_world_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[3];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_linear_accel_in_world(ldata);
-    data[0] = (float)ldata[0] / 65536.0f;
-    data[1] = (float)ldata[1] / 65536.0f;
-    data[2] = (float)ldata[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gravity_float is used to get an estimate of the body frame
- *          gravity vector, based on the most recent sensor fusion solution.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long at least</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gravity_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[3];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    result = inv_get_gravity(ldata);
-    data[0] = (float)ldata[0] / 65536.0f;
-    data[1] = (float)ldata[1] / 65536.0f;
-    data[2] = (float)ldata[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_cal_matrix_float is used to get the gyroscope
- *          calibration matrix. The gyroscope calibration matrix defines the relationship
- *          between the gyroscope sensor axes and the sensor fusion solution axes.
- *          Calibration matrix data members will have a value of 1.0, 0, or -1.0.
- *          The matrix has members
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_cal_matrix_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f;
-    data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f;
-    data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f;
-    data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f;
-    data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f;
-    data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f;
-    data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f;
-    data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f;
-    data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_accel_cal_matrix_float is used to get the accelerometer
- *          calibration matrix.
- *          Calibration matrix data members will have a value of 1.0, 0, or -1.0.
- *          The matrix has members
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-
-inv_error_t inv_get_accel_cal_matrix_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f;
-    data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f;
-    data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f;
-    data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f;
-    data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f;
-    data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f;
-    data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f;
-    data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f;
-    data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_cal_matrix_float is used to get an array of nine data points
- *			representing the calibration matrix for the compass:
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long at least</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_cal_matrix_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f;
-    data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f;
-    data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f;
-    data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f;
-    data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f;
-    data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f;
-    data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f;
-    data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f;
-    data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f;
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_temp_slope_float is used to get the temperature
- *          compensation algorithm's estimate of the gyroscope bias temperature
- *          coefficient.
- *          The argument array elements are ordered X,Y,Z.
- *          Values are in units of dps per deg C (degrees per second per degree
- *          Celcius)
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long </b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_temp_slope_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
-        data[0] = inv_obj.x_gyro_coef[1];
-        data[1] = inv_obj.y_gyro_coef[1];
-        data[2] = inv_obj.z_gyro_coef[1];
-    } else {
-        data[0] = (float)inv_obj.temp_slope[0] / 65536.0f;
-        data[1] = (float)inv_obj.temp_slope[1] / 65536.0f;
-        data[2] = (float)inv_obj.temp_slope[2] / 65536.0f;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_bias_float is used to get the gyroscope biases.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of dps (degrees per second).
-
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_bias_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f;
-    data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f;
-    data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_accel_bias_float is used to get the accelerometer baises.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of g (gravity).
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_accel_bias_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.accel_bias[0] / 65536.0f;
-    data[1] = (float)inv_obj.accel_bias[1] / 65536.0f;
-    data[2] = (float)inv_obj.accel_bias[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_bias_float is used to get an array of three data points representing
- *			the compass biases.
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_bias_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] =
-        ((float)
-         (inv_obj.compass_bias[0] +
-          (long)((long long)inv_obj.init_compass_bias[0] *
-                 inv_obj.compass_sens / 16384))) / 65536.0f;
-    data[1] =
-        ((float)
-         (inv_obj.compass_bias[1] +
-          (long)((long long)inv_obj.init_compass_bias[1] *
-                 inv_obj.compass_sens / 16384))) / 65536.0f;
-    data[2] =
-        ((float)
-         (inv_obj.compass_bias[2] +
-          (long)((long long)inv_obj.init_compass_bias[2] *
-                 inv_obj.compass_sens / 16384))) / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data.
- *          The argument array elements are ordered gyroscope X,Y, and Z,
- *          accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
- *          \if UMPL The magnetometer elements are not populated in UMPL. \endif
- *          The gyroscope and accelerometer data is not scaled or offset, it is
- *          copied directly from the sensor registers, and cast as a float.
- *          In the case of accelerometers with 8-bit output resolution, the data
- *          is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
- *          full scale range
-
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_and_accel_sensor_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    long ldata[6];
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_gyro_and_accel_sensor(ldata);
-    data[0] = (float)ldata[0];
-    data[1] = (float)ldata[1];
-    data[2] = (float)ldata[2];
-    data[3] = (float)ldata[3];
-    data[4] = (float)ldata[4];
-    data[5] = (float)ldata[5];
-    data[6] = (float)inv_obj.compass_sensor_data[0];
-    data[7] = (float)inv_obj.compass_sensor_data[1];
-    data[8] = (float)inv_obj.compass_sensor_data[2];
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_euler_angles_x is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          Euler angles are returned according to the X convention.
- *          This is typically the convention used for mobile devices where the X
- *          axis is the width of the screen, Y axis is the height, and Z the
- *          depth. In this case roll is defined as the rotation around the X
- *          axis of the device.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>Y axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
- *
-           </TABLE>
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_x_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[6];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (float)(atan2f(rotMatrix[7],
-                       rotMatrix[8]) * 57.29577951308);
-    data[1] = (float)((double)asin(tmp) * 57.29577951308);
-    data[2] =
-        (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308);
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_euler_angles_float is used to get an array of three data points three data points
- *			representing roll, pitch, and yaw corresponding to the INV_EULER_ANGLES_X output and it is
- *          therefore the default convention for Euler angles.
- *          Please refer to the INV_EULER_ANGLES_X for a detailed description.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_float(float *data)
-{
-    return inv_get_euler_angles_x_float(data);
-}
-
-/**  @brief  inv_get_euler_angles_y_float is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          Euler angles are returned according to the Y convention.
- *          This convention is typically used in augmented reality applications,
- *          where roll is defined as the rotation around the axis along the
- *          height of the screen of a mobile device, namely the Y axis.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Y axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
- *          </TABLE>
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_y_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[7];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308);
-    data[1] = (float)((double)asin(tmp) * 57.29577951308);
-    data[2] =
-        (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308);
-
-    return result;
-}
-
-/**  @brief  inv_get_euler_angles_z_float is used to get the Euler angle representation
- *          of the current sensor fusion solution.
- *          This convention is mostly used in application involving the use
- *          of a camera, typically placed on the back of a mobile device, that
- *          is along the Z axis.  In this convention roll is defined as the
- *          rotation around the Z axis.
- *          Euler angles are returned according to the Y convention.
- *          <TABLE>
- *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Z axis                </TD></TR>
- *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Y axis                </TD></TR>
- *          </TABLE>
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_z_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    result = inv_get_rot_mat_float(rotMatrix);
-    tmp = rotMatrix[8];
-    if (tmp > 1.0f) {
-        tmp = 1.0f;
-    }
-    if (tmp < -1.0f) {
-        tmp = -1.0f;
-    }
-    data[0] =
-        (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308);
-    data[1] = (float)((double)asin(tmp) * 57.29577951308);
-    data[2] =
-        (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308);
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_raw_data_float is used to get Raw magnetometer data
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_raw_data_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] =
-        (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]);
-    data[1] =
-        (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]);
-    data[2] =
-        (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]);
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_magnetometer_float is used to get magnetometer data
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_magnetometer_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f;
-    data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f;
-    data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_pressure_float is used to get a single value representing the pressure in Pascal
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to the data to be passed back to the user.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_pressure_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.pressure;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_heading_float is used to get single number representing the heading of the device
- *          relative to the Earth, in which 0 represents North, 90 degrees
- *          represents East, and so on.
- *          The heading is defined as the direction of the +Y axis if the Y
- *          axis is horizontal, and otherwise the direction of the -Z axis.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to the data to be passed back to the user.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_heading_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    float rotMatrix[9];
-    float tmp;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    inv_get_rot_mat_float(rotMatrix);
-    if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
-        tmp =
-            (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
-                    90.0f);
-    } else {
-        tmp =
-            (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
-                    90.0f);
-    }
-    if (tmp < 0) {
-        tmp += 360.0f;
-    }
-    data[0] = 360 - tmp;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_bias_error_float is used to get an array of three numbers representing
- *			the current estimated error in the compass biases. These numbers are unitless and serve
- *          as rough estimates in which numbers less than 100 typically represent
- *          reasonably well calibrated compass axes.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_bias_error_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    if (inv_obj.large_field == 0) {
-        data[0] = (float)inv_obj.compass_bias_error[0];
-        data[1] = (float)inv_obj.compass_bias_error[1];
-        data[2] = (float)inv_obj.compass_bias_error[2];
-    } else {
-        data[0] = (float)P_INIT;
-        data[1] = (float)P_INIT;
-        data[2] = (float)P_INIT;
-    }
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_mag_scale_float is used to get magnetometer scale.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_mag_scale_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.compass_scale[0] / 65536.0f;
-    data[1] = (float)inv_obj.compass_scale[1] / 65536.0f;
-    data[2] = (float)inv_obj.compass_scale[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_local_field_float is used to get local magnetic field data.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 3 cells long</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_local_field_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.local_field[0] / 65536.0f;
-    data[1] = (float)inv_obj.local_field[1] / 65536.0f;
-    data[2] = (float)inv_obj.local_field[2] / 65536.0f;
-
-    return result;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_get_relative_quaternion_float is used to get relative quaternion data.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *          must have been called.
- *
- *  @param  data
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 4 cells long at least</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- *  @endcond
- */
-inv_error_t inv_get_relative_quaternion_float(float *data)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f;
-    data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f;
-    data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f;
-    data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f;
-
-    return result;
-}
-
-/**
- * Returns the curren compass accuracy.
- *
- * - 0: Unknown: The accuracy is unreliable and compass data should not be used
- * - 1: Low: The compass accuracy is low.
- * - 2: Medium: The compass accuracy is medium.
- * - 3: High: The compas acurracy is high and can be trusted
- *
- * @param accuracy The accuracy level in the range 0-3
- *
- * @return ML_SUCCESS or non-zero error code
- */
-inv_error_t inv_get_compass_accuracy(int *accuracy)
-{
-    if (inv_get_state() != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    *accuracy = inv_obj.compass_accuracy;
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_gyro_bias is used to set the gyroscope bias.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled at 1 dps = 2^16 LSBs.
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_bias(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-    long biasTmp;
-    long sf = 0;
-    short offset[GYRO_NUM_AXES];
-    int i;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    if (mldl_cfg->gyro_sens_trim != 0) {
-        sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
-    } else {
-        sf = 2000;
-    }
-    for (i = 0; i < GYRO_NUM_AXES; i++) {
-        inv_obj.gyro_bias[i] = data[i];
-        biasTmp = -inv_obj.gyro_bias[i] / sf;
-        if (biasTmp < 0)
-            biasTmp += 65536L;
-        offset[i] = (short)biasTmp;
-    }
-    result = inv_set_offset(offset);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_accel_bias is used to set the accelerometer bias.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are scaled in units of g (gravity),
- *          where 1 g = 2^16 LSBs.
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_accel_bias(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-    long biasTmp;
-    int i, j;
-    unsigned char regs[6];
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    for (i = 0; i < ACCEL_NUM_AXES; i++) {
-        inv_obj.accel_bias[i] = data[i];
-        if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) {
-            long long tmp64;
-            inv_obj.scaled_accel_bias[i] = 0;
-            for (j = 0; j < ACCEL_NUM_AXES; j++) {
-                inv_obj.scaled_accel_bias[i] +=
-                    data[j] *
-                    (long)mldl_cfg->pdata->accel.orientation[i * 3 + j];
-            }
-            tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13;
-            biasTmp = (long)(tmp64 / inv_obj.accel_sens);
-        } else {
-            biasTmp = 0;
-        }
-        if (biasTmp < 0)
-            biasTmp += 65536L;
-        regs[2 * i + 0] = (unsigned char)(biasTmp / 256);
-        regs[2 * i + 1] = (unsigned char)(biasTmp % 256);
-    }
-    result = inv_set_mpu_memory(KEY_D_1_8, 2, &regs[0]);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_mpu_memory(KEY_D_1_10, 2, &regs[2]);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_set_mpu_memory(KEY_D_1_2, 2, &regs[4]);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_mag_bias is used to set Compass Bias
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_mag_bias(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    inv_set_compass_bias(data);
-    inv_obj.init_compass_bias[0] = 0;
-    inv_obj.init_compass_bias[1] = 0;
-    inv_obj.init_compass_bias[2] = 0;
-    inv_obj.got_compass_bias = 1;
-    inv_obj.got_init_compass_bias = 1;
-    inv_obj.compass_state = SF_STARTUP_SETTLE;
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_gyro_temp_slope is used to set the temperature
- *          compensation algorithm's estimate of the gyroscope bias temperature
- *          coefficient.
- *          The argument array elements are ordered X,Y,Z.
- *          Values are in units of dps per deg C (degrees per second per degree
- *          Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs.
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document.
- *
- *  @brief  inv_set_gyro_temp_slope is used to set Gyro temperature slope
- *
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_temp_slope(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-    int i;
-    long sf;
-    unsigned char regs[3];
-
-    inv_obj.factory_temp_comp = 1;
-    inv_obj.temp_slope[0] = data[0];
-    inv_obj.temp_slope[1] = data[1];
-    inv_obj.temp_slope[2] = data[2];
-    for (i = 0; i < GYRO_NUM_AXES; i++) {
-        sf = -inv_obj.temp_slope[i] / 1118;
-        if (sf > 127) {
-            sf -= 256;
-        }
-        regs[i] = (unsigned char)sf;
-    }
-    result = inv_set_offsetTC(regs);
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_local_field is used to set local magnetic field
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_local_field(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    inv_obj.local_field[0] = data[0];
-    inv_obj.local_field[1] = data[1];
-    inv_obj.local_field[2] = data[2];
-    inv_obj.new_local_field = 1;
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_mag_scale is used to set magnetometer scale
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_mag_scale(long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    inv_obj.compass_scale[0] = data[0];
-    inv_obj.compass_scale[1] = data[1];
-    inv_obj.compass_scale[2] = data[2];
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_gyro_temp_slope_float is used to get the temperature
- *          compensation algorithm's estimate of the gyroscope bias temperature
- *          coefficient.
- *          The argument array elements are ordered X,Y,Z.
- *          Values are in units of dps per deg C (degrees per second per degree
- *          Celcius)
-
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_temp_slope_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_gyro_temp_slope(arrayTmp);
-}
-
-/**
- *  @brief  inv_set_gyro_bias_float is used to set the gyroscope bias.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of dps (degrees per second).
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_bias_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_gyro_bias(arrayTmp);
-
-}
-
-/**
- *  @brief  inv_set_accel_bias_float is used to set the accelerometer bias.
- *          The argument array elements are ordered X,Y,Z.
- *          The values are in units of g (gravity).
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_accel_bias_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_accel_bias(arrayTmp);
-
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_mag_bias_float is used to set compass bias
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen()\ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_mag_bias_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_mag_bias(arrayTmp);
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_local_field_float is used to set local magnetic field
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_local_field_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_local_field(arrayTmp);
-}
-
-/**
- *  @cond MPL
- *  @brief  inv_set_mag_scale_float is used to set magnetometer scale
- *
- *          Please refer to the provided "9-Axis Sensor Fusion
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() \ifnot UMPL or
- *          MLDmpPedometerStandAloneOpen() \endif
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- *  @endcond
- */
-inv_error_t inv_set_mag_scale_float(float *data)
-{
-    long arrayTmp[3];
-    arrayTmp[0] = (long)(data[0] * 65536.f);
-    arrayTmp[1] = (long)(data[1] * 65536.f);
-    arrayTmp[2] = (long)(data[2] * 65536.f);
-    return inv_set_mag_scale(arrayTmp);
-}
diff --git a/mlsdk/mllite/mlarray_legacy.c b/mlsdk/mllite/mlarray_legacy.c
deleted file mode 100644
index 9dce8f3..0000000
--- a/mlsdk/mllite/mlarray_legacy.c
+++ /dev/null
@@ -1,587 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- *
- * $Id: mlarray_legacy.c $
- *
- *****************************************************************************/
-
-/** 
- *  @defgroup MLArray_Legacy 
- *  @brief  Legacy Motion Library Array APIs.
- *          The Motion Library Array APIs provide the user access to the
- *          Motion Library state. These Legacy APIs provide access to
- *          individual state arrays using a data set name as the first
- *          argument to the API. This format has been replaced by unique
- *          named APIs for each data set, found in the MLArray group.
- *
- *  @{
- *      @file   mlarray_legacy.c
- *      @brief  The Legacy Motion Library Array APIs.
- */
-
-#include "ml.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mlFIFO.h"
-#include "mldl_cfg.h"
-
-/**
- *  @brief  inv_get_array is used to get an array of processed motion sensor data.
- *          inv_get_array can be used to retrieve various data sets. Certain data
- *          sets require functions to be enabled using MLEnable in order to be
- *          valid.
- *
- *          The available data sets are:
- *
- *          - INV_ROTATION_MATRIX
- *          - INV_QUATERNION
- *          - INV_EULER_ANGLES_X
- *          - INV_EULER_ANGLES_Y
- *          - INV_EULER_ANGLES_Z
- *          - INV_EULER_ANGLES
- *          - INV_LINEAR_ACCELERATION
- *          - INV_LINEAR_ACCELERATION_WORLD
- *          - INV_GRAVITY
- *          - INV_ANGULAR_VELOCITY
- *          - INV_RAW_DATA
- *          - INV_GYROS
- *          - INV_ACCELS
- *          - INV_MAGNETOMETER
- *          - INV_GYRO_BIAS
- *          - INV_ACCEL_BIAS
- *          - INV_MAG_BIAS 
- *          - INV_HEADING
- *          - INV_MAG_BIAS_ERROR
- *          - INV_PRESSURE
- *
- *          Please refer to the documentation of inv_get_float_array() for a 
- *          description of these data sets.
- *
- *  @pre    MLDmpOpen() or MLDmpPedometerStandAloneOpen()
- *          must have been called.
- *
- *  @param  dataSet     
- *              A constant specifying an array of data processed by the
- *              motion processor.
- *  @param  data        
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long at least</b>.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_array(int dataSet, long *data)
-{
-    inv_error_t result;
-    switch (dataSet) {
-    case INV_GYROS:
-        result = inv_get_gyro(data);
-        break;
-    case INV_ACCELS:
-        result = inv_get_accel(data);
-        break;
-    case INV_TEMPERATURE:
-        result = inv_get_temperature(data);
-        break;
-    case INV_ROTATION_MATRIX:
-        result = inv_get_rot_mat(data);
-        break;
-    case INV_QUATERNION:
-        result = inv_get_quaternion(data);
-        break;
-    case INV_LINEAR_ACCELERATION:
-        result = inv_get_linear_accel(data);
-        break;
-    case INV_LINEAR_ACCELERATION_WORLD:
-        result = inv_get_linear_accel_in_world(data);
-        break;
-    case INV_GRAVITY:
-        result = inv_get_gravity(data);
-        break;
-    case INV_ANGULAR_VELOCITY:
-        result = inv_get_angular_velocity(data);
-        break;
-    case INV_EULER_ANGLES:
-        result = inv_get_euler_angles(data);
-        break;
-    case INV_EULER_ANGLES_X:
-        result = inv_get_euler_angles_x(data);
-        break;
-    case INV_EULER_ANGLES_Y:
-        result = inv_get_euler_angles_y(data);
-        break;
-    case INV_EULER_ANGLES_Z:
-        result = inv_get_euler_angles_z(data);
-        break;
-    case INV_GYRO_TEMP_SLOPE:
-        result = inv_get_gyro_temp_slope(data);
-        break;
-    case INV_GYRO_BIAS:
-        result = inv_get_gyro_bias(data);
-        break;
-    case INV_ACCEL_BIAS:
-        result = inv_get_accel_bias(data);
-        break;
-    case INV_MAG_BIAS:
-        result = inv_get_mag_bias(data);
-        break;
-    case INV_RAW_DATA:
-        result = inv_get_gyro_and_accel_sensor(data);
-        break;
-    case INV_MAG_RAW_DATA:
-        result = inv_get_mag_raw_data(data);
-        break;
-    case INV_MAGNETOMETER:
-        result = inv_get_magnetometer(data);
-        break;
-    case INV_PRESSURE:
-        result = inv_get_pressure(data);
-        break;
-    case INV_HEADING:
-        result = inv_get_heading(data);
-        break;
-    case INV_GYRO_CALIBRATION_MATRIX:
-        result = inv_get_gyro_cal_matrix(data);
-        break;
-    case INV_ACCEL_CALIBRATION_MATRIX:
-        result = inv_get_accel_cal_matrix(data);
-        break;
-    case INV_MAG_CALIBRATION_MATRIX:
-        result = inv_get_mag_cal_matrix(data);
-        break;
-    case INV_MAG_BIAS_ERROR:
-        result = inv_get_mag_bias_error(data);
-        break;
-    case INV_MAG_SCALE:
-        result = inv_get_mag_scale(data);
-        break;
-    case INV_LOCAL_FIELD:
-        result = inv_get_local_field(data);
-        break;
-    case INV_RELATIVE_QUATERNION:
-        result = inv_get_relative_quaternion(data);
-        break;
-    default:
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-    return result;
-}
-
-/**
- *  @brief  inv_get_float_array is used to get an array of processed motion sensor 
- *          data. inv_get_array can be used to retrieve various data sets. 
- *          Certain data sets require functions to be enabled using MLEnable 
- *          in order to be valid.
- *
- *          The available data sets are:
- *
- *          - INV_ROTATION_MATRIX :
- *          Returns an array of nine data points representing the rotation 
- *          matrix generated from all available sensors. 
- *          This requires that ML_SENSOR_FUSION be enabled.
- *          The array format will be R11, R12, R13, R21, R22, R23, R31, R32, 
- *          R33, representing the matrix:
- *          <center>R11 R12 R13</center>
- *          <center>R21 R22 R23</center>
- *          <center>R31 R32 R33</center>
- *          <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, 
- *          section 7 "Sensor Fusion Output", for details regarding rotation 
- *          matrix output</b>.
- *
- *          - INV_QUATERNION :
- *          Returns an array of four data points representing the quaternion 
- *          generated from all available sensors. 
- *          This requires that ML_SENSOR_FUSION be enabled.
- *
- *          - INV_EULER_ANGLES_X :
- *          Returns an array of three data points representing roll, pitch, and 
- *          yaw using the X axis of the gyroscope, accelerometer, and compass as 
- *          reference axis. 
- *          This is typically the convention used for mobile devices where the X
- *          axis is the width of the screen, Y axis is the height, and Z the 
- *          depth. In this case roll is defined as the rotation around the X 
- *          axis of the device.
- *          The euler angles convention for this output is the following:
- *          <TABLE>
- *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
- *          <TR><TD>roll              </TD><TD>X axis                </TD></TR>
- *          <TR><TD>pitch             </TD><TD>Y axis                </TD></TR>
- *          <TR><TD>yaw               </TD><TD>Z axis                </TD></TR>
- *          </TABLE>
- *          INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is 
- *          therefore the default convention.
- *
- *          - INV_EULER_ANGLES_Y :
- *          Returns an array of three data points representing roll, pitch, and 
- *          yaw using the Y axis of the gyroscope, accelerometer, and compass as 
- *          reference axis. 
- *          This convention is typically used in augmented reality applications, 
- *          where roll is defined as the rotation around the axis along the 
- *          height of the screen of a mobile device, namely the Y axis.
- *          The euler angles convention for this output is the following:
- *          <TABLE>
- *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
- *          <TR><TD>roll              </TD><TD>Y axis                </TD></TR>
- *          <TR><TD>pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD>yaw               </TD><TD>Z axis                </TD></TR>
- *          </TABLE>
- *
- *          - INV_EULER_ANGLES_Z :
- *          Returns an array of three data points representing roll, pitch, and 
- *          yaw using the Z axis of the gyroscope, accelerometer, and compass as 
- *          reference axis. 
- *          This convention is mostly used in application involving the use 
- *          of a camera, typically placed on the back of a mobile device, that 
- *          is along the Z axis.  In this convention roll is defined as the 
- *          rotation around the Z axis.
- *          The euler angles convention for this output is the following:
- *          <TABLE>
- *          <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR>
- *          <TR><TD>roll              </TD><TD>Z axis                </TD></TR>
- *          <TR><TD>pitch             </TD><TD>X axis                </TD></TR>
- *          <TR><TD>yaw               </TD><TD>Y axis                </TD></TR>
- *          </TABLE>
- *
- *          - INV_EULER_ANGLES :
- *          Returns an array of three data points representing roll, pitch, and 
- *          yaw corresponding to the INV_EULER_ANGLES_X output and it is 
- *          therefore the default convention for Euler angles.
- *          Please refer to the INV_EULER_ANGLES_X for a detailed description.
- *
- *          - INV_LINEAR_ACCELERATION :
- *          Returns an array of three data points representing the linear 
- *          acceleration as derived from both gyroscopes and accelerometers. 
- *          This requires that ML_SENSOR_FUSION be enabled.
- *
- *          - INV_LINEAR_ACCELERATION_WORLD :
- *          Returns an array of three data points representing the linear 
- *          acceleration in world coordinates, as derived from both gyroscopes 
- *          and accelerometers.
- *          This requires that ML_SENSOR_FUSION be enabled.
- *
- *          - INV_GRAVITY :
- *          Returns an array of three data points representing the direction 
- *          of gravity in body coordinates, as derived from both gyroscopes 
- *          and accelerometers.
- *          This requires that ML_SENSOR_FUSION be enabled.
- *
- *          - INV_ANGULAR_VELOCITY :
- *          Returns an array of three data points representing the angular 
- *          velocity as derived from <b>both</b> gyroscopes and accelerometers.
- *          This requires that ML_SENSOR_FUSION be enabled, to fuse data from
- *          the gyroscope and accelerometer device, appropriately scaled and 
- *          oriented according to the respective mounting matrices.
- *
- *          - INV_RAW_DATA :
- *          Returns an array of nine data points representing raw sensor data 
- *          of the gyroscope X, Y, Z, accelerometer X, Y, Z, and 
- *          compass X, Y, Z values.
- *          These values are not scaled and come out directly from the devices'
- *          sensor data output. In case of accelerometers with lower output 
- *          resolution, e.g 8-bit, the sensor data is scaled up to match the 
- *          2^14 = 1 gee typical representation for a +/- 2 gee full scale 
- *          range.
- *
- *          - INV_GYROS :
- *          Returns an array of three data points representing the X gyroscope,
- *          Y gyroscope, and Z gyroscope values.
- *          The values are not sensor fused with other sensor types data but
- *          reflect the orientation from the mounting matrices in use.
- *          The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 
- *          codes.
- *
- *          - INV_ACCELS :
- *          Returns an array of three data points representing the X 
- *          accelerometer, Y accelerometer, and Z accelerometer values.
- *          The values are not sensor fused with other sensor types data but
- *          reflect the orientation from the mounting matrices in use.
- *          The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16
- *          codes.
- *
- *          - INV_MAGNETOMETER :
- *          Returns an array of three data points representing the compass
- *          X, Y, and Z values.
- *          The values are not sensor fused with other sensor types data but
- *          reflect the orientation from the mounting matrices in use.
- *          The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) 
- *          corresponds to 2^16 codes.
- *
- *          - INV_GYRO_BIAS :
- *          Returns an array of three data points representing the gyroscope 
- *          biases.
- *
- *          - INV_ACCEL_BIAS :
- *          Returns an array of three data points representing the 
- *          accelerometer biases.
- *
- *          - INV_MAG_BIAS :
- *          Returns an array of three data points representing the compass 
- *          biases.
- *
- *          - INV_GYRO_CALIBRATION_MATRIX :
- *          Returns an array of nine data points representing the calibration 
- *          matrix for the gyroscopes:
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *
- *          - INV_ACCEL_CALIBRATION_MATRIX :
- *          Returns an array of nine data points representing the calibration 
- *          matrix for the accelerometers:
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *
- *          - INV_MAG_CALIBRATION_MATRIX :
- *          Returns an array of nine data points representing the calibration 
- *          matrix for the compass:
- *          <center>C11 C12 C13</center>
- *          <center>C21 C22 C23</center>
- *          <center>C31 C32 C33</center>
- *
- *          - INV_PRESSURE :
- *          Returns a single value representing the pressure in Pascal
- *
- *          - INV_HEADING : 
- *          Returns a single number representing the heading of the device 
- *          relative to the Earth, in which 0 represents North, 90 degrees 
- *          represents East, and so on. 
- *          The heading is defined as the direction of the +Y axis if the Y 
- *          axis is horizontal, and otherwise the direction of the -Z axis.
- *
- *          - INV_MAG_BIAS_ERROR :
- *          Returns an array of three numbers representing the current estimated
- *          error in the compass biases. These numbers are unitless and serve
- *          as rough estimates in which numbers less than 100 typically represent
- *          reasonably well calibrated compass axes.
- *
- *  @pre    MLDmpOpen() or MLDmpPedometerStandAloneOpen()
- *          must have been called.
- *
- *  @param  dataSet     
- *              A constant specifying an array of data processed by 
- *              the motion processor.
- *  @param  data        
- *              A pointer to an array to be passed back to the user.
- *              <b>Must be 9 cells long at least</b>.
- *
- *  @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_float_array(int dataSet, float *data)
-{
-    inv_error_t result;
-    switch (dataSet) {
-    case INV_GYROS:
-        result = inv_get_gyro_float(data);
-        break;
-    case INV_ACCELS:
-        result = inv_get_accel_float(data);
-        break;
-    case INV_TEMPERATURE:
-        result = inv_get_temperature_float(data);
-        break;
-    case INV_ROTATION_MATRIX:
-        result = inv_get_rot_mat_float(data);
-        break;
-    case INV_QUATERNION:
-        result = inv_get_quaternion_float(data);
-        break;
-    case INV_LINEAR_ACCELERATION:
-        result = inv_get_linear_accel_float(data);
-        break;
-    case INV_LINEAR_ACCELERATION_WORLD:
-        result = inv_get_linear_accel_in_world_float(data);
-        break;
-    case INV_GRAVITY:
-        result = inv_get_gravity_float(data);
-        break;
-    case INV_ANGULAR_VELOCITY:
-        result = inv_get_angular_velocity_float(data);
-        break;
-    case INV_EULER_ANGLES:
-        result = inv_get_euler_angles_float(data);
-        break;
-    case INV_EULER_ANGLES_X:
-        result = inv_get_euler_angles_x_float(data);
-        break;
-    case INV_EULER_ANGLES_Y:
-        result = inv_get_euler_angles_y_float(data);
-        break;
-    case INV_EULER_ANGLES_Z:
-        result = inv_get_euler_angles_z_float(data);
-        break;
-    case INV_GYRO_TEMP_SLOPE:
-        result = inv_get_gyro_temp_slope_float(data);
-        break;
-    case INV_GYRO_BIAS:
-        result = inv_get_gyro_bias_float(data);
-        break;
-    case INV_ACCEL_BIAS:
-        result = inv_get_accel_bias_float(data);
-        break;
-    case INV_MAG_BIAS:
-        result = inv_get_mag_bias_float(data);
-        break;
-    case INV_RAW_DATA:
-        result = inv_get_gyro_and_accel_sensor_float(data);
-        break;
-    case INV_MAG_RAW_DATA:
-        result = inv_get_mag_raw_data_float(data);
-        break;
-    case INV_MAGNETOMETER:
-        result = inv_get_magnetometer_float(data);
-        break;
-    case INV_PRESSURE:
-        result = inv_get_pressure_float(data);
-        break;
-    case INV_HEADING:
-        result = inv_get_heading_float(data);
-        break;
-    case INV_GYRO_CALIBRATION_MATRIX:
-        result = inv_get_gyro_cal_matrix_float(data);
-        break;
-    case INV_ACCEL_CALIBRATION_MATRIX:
-        result = inv_get_accel_cal_matrix_float(data);
-        break;
-    case INV_MAG_CALIBRATION_MATRIX:
-        result = inv_get_mag_cal_matrix_float(data);
-        break;
-    case INV_MAG_BIAS_ERROR:
-        result = inv_get_mag_bias_error_float(data);
-        break;
-    case INV_MAG_SCALE:
-        result = inv_get_mag_scale_float(data);
-        break;
-    case INV_LOCAL_FIELD:
-        result = inv_get_local_field_float(data);
-        break;
-    case INV_RELATIVE_QUATERNION:
-        result = inv_get_relative_quaternion_float(data);
-        break;
-    default:
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-    return result;
-}
-
-/**
- *  @brief  used to set an array of motion sensor data.
- *          Handles the following data sets:
- *          - INV_GYRO_BIAS
- *          - INV_ACCEL_BIAS
- *          - INV_MAG_BIAS
- *          - INV_GYRO_TEMP_SLOPE
- *
- *          For more details about the use of the data sets
- *          please refer to the documentation of inv_set_float_array().
- *
- *          Please also refer to the provided "9-Axis Sensor Fusion 
- *          Application Note" document provided.
- *
- *  @pre    MLDmpOpen() or 
- *          MLDmpPedometerStandAloneOpen() 
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  dataSet     A constant specifying an array of data.
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_array(int dataSet, long *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    switch (dataSet) {
-    case INV_GYRO_BIAS:
-        result = inv_set_gyro_bias(data);
-        break;
-    case INV_ACCEL_BIAS:
-        result = inv_set_accel_bias(data);
-        break;
-    case INV_MAG_BIAS:
-        result = inv_set_mag_bias(data);
-        break;
-    case INV_GYRO_TEMP_SLOPE:
-        result = inv_set_gyro_temp_slope(data);
-        break;
-    case INV_LOCAL_FIELD:
-        result = inv_set_local_field(data);
-        break;
-    case INV_MAG_SCALE:
-        result = inv_set_mag_scale(data);
-        break;
-    default:
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-    return result;
-}
-
-/**
- *  @brief  used to set an array of motion sensor data.
- *          Handles various data sets:
- *          - INV_GYRO_BIAS
- *          - INV_ACCEL_BIAS
- *          - INV_MAG_BIAS
- *          - INV_GYRO_TEMP_SLOPE
- *
- *          Please refer to the provided "9-Axis Sensor Fusion Application
- *          Note" document provided.
- *
- *  @pre    MLDmpOpen() or 
- *          MLDmpPedometerStandAloneOpen() 
- *  @pre    MLDmpStart() must <b>NOT</b> have been called.
- *
- *  @param  dataSet     A constant specifying an array of data.
- *  @param  data        A pointer to an array to be copied from the user.
- *
- *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_float_array(int dataSet, float *data)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    switch (dataSet) {
-    case INV_GYRO_TEMP_SLOPE:  // internal
-        result = inv_set_gyro_temp_slope_float(data);
-        break;
-    case INV_GYRO_BIAS:        // internal
-        result = inv_set_gyro_bias_float(data);
-        break;
-    case INV_ACCEL_BIAS:       // internal
-        result = inv_set_accel_bias_float(data);
-        break;
-    case INV_MAG_BIAS:         // internal            
-        result = inv_set_mag_bias_float(data);
-        break;
-    case INV_LOCAL_FIELD:      // internal     
-        result = inv_set_local_field_float(data);
-        break;
-    case INV_MAG_SCALE:        // internal            
-        result = inv_set_mag_scale_float(data);
-        break;
-    default:
-        result = INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return result;
-}
diff --git a/mlsdk/mllite/mlcontrol.c b/mlsdk/mllite/mlcontrol.c
deleted file mode 100644
index bd186fa..0000000
--- a/mlsdk/mllite/mlcontrol.c
+++ /dev/null
@@ -1,797 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
- 
-/*******************************************************************************
- *
- * $Id: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- *  @defgroup   CONTROL
- *  @brief      Motion Library - Control Engine.
- *              The Control Library processes gyroscopes, accelerometers, and 
- *              compasses to provide control signals that can be used in user 
- *              interfaces.
- *              These signals can be used to manipulate objects such as documents,
- *              images, cursors, menus, etc.
- *
- *  @{
- *      @file   mlcontrol.c
- *      @brief  The Control Library.
- *
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mltypes.h"
-#include "ml.h"
-#include "mlos.h"
-#include "mlsl.h"
-#include "mldl.h"
-#include "mlcontrol.h"
-#include "dmpKey.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "string.h"
-
-/* - Global Vars. - */
-struct control_params cntrl_params = {
-    {
-     MLCTRL_SENSITIVITY_0_DEFAULT,
-     MLCTRL_SENSITIVITY_1_DEFAULT,
-     MLCTRL_SENSITIVITY_2_DEFAULT,
-     MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity
-    MLCTRL_FUNCTIONS_DEFAULT,   // functions
-    {
-     MLCTRL_PARAMETER_ARRAY_0_DEFAULT,
-     MLCTRL_PARAMETER_ARRAY_1_DEFAULT,
-     MLCTRL_PARAMETER_ARRAY_2_DEFAULT,
-     MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray
-    {
-     MLCTRL_PARAMETER_AXIS_0_DEFAULT,
-     MLCTRL_PARAMETER_AXIS_1_DEFAULT,
-     MLCTRL_PARAMETER_AXIS_2_DEFAULT,
-     MLCTRL_PARAMETER_AXIS_3_DEFAULT},  // parameterAxis
-    {
-     MLCTRL_GRID_THRESHOLD_0_DEFAULT,
-     MLCTRL_GRID_THRESHOLD_1_DEFAULT,
-     MLCTRL_GRID_THRESHOLD_2_DEFAULT,
-     MLCTRL_GRID_THRESHOLD_3_DEFAULT},  // gridThreshold
-    {
-     MLCTRL_GRID_MAXIMUM_0_DEFAULT,
-     MLCTRL_GRID_MAXIMUM_1_DEFAULT,
-     MLCTRL_GRID_MAXIMUM_2_DEFAULT,
-     MLCTRL_GRID_MAXIMUM_3_DEFAULT},    // gridMaximum
-    MLCTRL_GRID_CALLBACK_DEFAULT    // gridCallback
-};
-
-/* - Extern Vars. - */
-struct control_obj cntrl_obj;
-extern const unsigned char *dmpConfig1;
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- *  @brief  inv_set_control_sensitivity is used to set the sensitivity for a control
- *          signal.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param controlSignal    Indicates which control signal is being modified.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 or
- *                          - INV_CONTROL_4.
- *
- *  @param sensitivity      The sensitivity of the control signal.
- *
- *  @return error code
- */
-inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
-                                        long sensitivity)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[2];
-    long finalSens = 0;
-    inv_error_t result;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    finalSens = sensitivity * 100;
-    if (finalSens > 16384) {
-        finalSens = 16384;
-    }
-    regs[0] = (unsigned char)(finalSens / 256);
-    regs[1] = (unsigned char)(finalSens % 256);
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        result = inv_set_mpu_memory(KEY_D_0_224, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        cntrl_params.sensitivity[0] = (unsigned short)sensitivity;
-        break;
-    case INV_CONTROL_2:
-        result = inv_set_mpu_memory(KEY_D_0_228, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        cntrl_params.sensitivity[1] = (unsigned short)sensitivity;
-        break;
-    case INV_CONTROL_3:
-        result = inv_set_mpu_memory(KEY_D_0_232, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        cntrl_params.sensitivity[2] = (unsigned short)sensitivity;
-        break;
-    case INV_CONTROL_4:
-        result = inv_set_mpu_memory(KEY_D_0_236, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        cntrl_params.sensitivity[3] = (unsigned short)sensitivity;
-        break;
-    default:
-        break;
-    }
-    if (finalSens != sensitivity * 100) {
-        return INV_ERROR_INVALID_PARAMETER;
-    } else {
-        return INV_SUCCESS;
-    }
-}
-
-/**
- *  @brief  inv_set_control_func allows the user to choose how the sensor data will
- *          be processed in order to provide a control parameter.
- *          inv_set_control_func allows the user to choose which control functions
- *          will be incorporated in the sensor data processing.
- *          The control functions are:
- *          - INV_GRID
- *          Indicates that the user will be controlling a system that
- *          has discrete steps, such as icons, menu entries, pixels, etc.
- *          - INV_SMOOTH
- *          Indicates that noise from unintentional motion should be filtered out.
- *          - INV_DEAD_ZONE
- *          Indicates that a dead zone should be used, below which sensor
- *          data is set to zero.
- *          - INV_HYSTERESIS
- *          Indicates that, when INV_GRID is selected, hysteresis should
- *          be used to prevent the control signal from switching rapidly across
- *          elements of the grid.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param  function    Indicates what functions will be used.
- *                      Can be a bitwise OR of several values.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_set_control_func(unsigned short function)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[8] = { DINA06, DINA26,
-        DINA46, DINA66,
-        DINA0E, DINA2E,
-        DINA4E, DINA6E
-    };
-    unsigned char i;
-    inv_error_t result;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if ((function & INV_SMOOTH) == 0) {
-        for (i = 0; i < 8; i++) {
-            regs[i] = DINA80 + 3;
-        }
-    }
-    result = inv_set_mpu_memory(KEY_CFG_4, 8, regs);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    cntrl_params.functions = function;
-    result = inv_set_dead_zone();
-
-    return result;
-}
-
-/**
- *  @brief  inv_get_control_signal is used to get the current control signal with
- *          high precision.
- *          inv_get_control_signal is used to acquire the current data of a control signal.
- *          If INV_GRID is being used, inv_get_grid_number will probably be preferrable.
- *
- *  @param  controlSignal   Indicates which control signal is being queried.
- *          Must be one of:
- *          - INV_CONTROL_1,
- *          - INV_CONTROL_2,
- *          - INV_CONTROL_3 or
- *          - INV_CONTROL_4.
- *
- *  @param  reset   Indicates whether the control signal should be reset to zero.
- *                  Options are INV_RESET or INV_NO_RESET
- *  @param  data    A pointer to the current control signal data.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_control_signal(unsigned short controlSignal,
-                                   unsigned short reset, long *data)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        *data = cntrl_obj.controlInt[0];
-        if (reset == INV_RESET) {
-            cntrl_obj.controlInt[0] = 0;
-        }
-        break;
-    case INV_CONTROL_2:
-        *data = cntrl_obj.controlInt[1];
-        if (reset == INV_RESET) {
-            cntrl_obj.controlInt[1] = 0;
-        }
-        break;
-    case INV_CONTROL_3:
-        *data = cntrl_obj.controlInt[2];
-        if (reset == INV_RESET) {
-            cntrl_obj.controlInt[2] = 0;
-        }
-        break;
-    case INV_CONTROL_4:
-        *data = cntrl_obj.controlInt[3];
-        if (reset == INV_RESET) {
-            cntrl_obj.controlInt[3] = 0;
-        }
-        break;
-    default:
-        break;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_get_grid_num is used to get the current grid location for a certain
- *          control signal.
- *          inv_get_grid_num is used to acquire the current grid location.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param  controlSignal   Indicates which control signal is being queried.
- *          Must be one of:
- *          - INV_CONTROL_1,
- *          - INV_CONTROL_2,
- *          - INV_CONTROL_3 or
- *          - INV_CONTROL_4.
- *
- *  @param  reset   Indicates whether the control signal should be reset to zero.
- *                  Options are INV_RESET or INV_NO_RESET
- *  @param  data    A pointer to the current grid number.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-
-inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset,
-                             long *data)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        *data = cntrl_obj.gridNum[0];
-        if (reset == INV_RESET) {
-            cntrl_obj.gridNum[0] = 0;
-        }
-        break;
-    case INV_CONTROL_2:
-        *data = cntrl_obj.gridNum[1];
-        if (reset == INV_RESET) {
-            cntrl_obj.gridNum[1] = 0;
-        }
-        break;
-    case INV_CONTROL_3:
-        *data = cntrl_obj.gridNum[2];
-        if (reset == INV_RESET) {
-            cntrl_obj.gridNum[2] = 0;
-        }
-        break;
-    case INV_CONTROL_4:
-        *data = cntrl_obj.gridNum[3];
-        if (reset == INV_RESET) {
-            cntrl_obj.gridNum[3] = 0;
-        }
-        break;
-    default:
-        break;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_grid_thresh is used to set the grid size for a control signal.
- *          inv_set_grid_thresh is used to adjust the size of the grid being controlled.
- *  @param  controlSignal   Indicates which control signal is being modified.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 and
- *                          - INV_CONTROL_4.
- *  @param  threshold       The threshold of the control signal at which the grid
- *                          number will be incremented or decremented.
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-
-inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() < INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        cntrl_params.gridThreshold[0] = threshold;
-        break;
-    case INV_CONTROL_2:
-        cntrl_params.gridThreshold[1] = threshold;
-        break;
-    case INV_CONTROL_3:
-        cntrl_params.gridThreshold[2] = threshold;
-        break;
-    case INV_CONTROL_4:
-        cntrl_params.gridThreshold[3] = threshold;
-        break;
-    default:
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_grid_max is used to set the maximum grid number for a control signal.
- *          inv_set_grid_max is used to adjust the maximum allowed grid number, above
- *          which the grid number will not be incremented.
- *          The minimum grid number is always zero.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param controlSignal    Indicates which control signal is being modified.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 and
- *                          - INV_CONTROL_4.
- *
- *  @param  maximum         The maximum grid number for a control signal.
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-
-inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        cntrl_params.gridMaximum[0] = maximum;
-        break;
-    case INV_CONTROL_2:
-        cntrl_params.gridMaximum[1] = maximum;
-        break;
-    case INV_CONTROL_3:
-        cntrl_params.gridMaximum[2] = maximum;
-        break;
-    case INV_CONTROL_4:
-        cntrl_params.gridMaximum[3] = maximum;
-        break;
-    default:
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  GridCallback function pointer type, to be passed as argument of 
- *          inv_set_grid_callback.
- *
- *  @param  controlSignal   Indicates which control signal crossed a grid threshold.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 and
- *                          - INV_CONTROL_4.
- *
- *  @param  gridNumber  An array of four numbers representing the grid number for each
- *                      control signal.
- *  @param  gridChange  An array of four numbers representing the change in grid number
- *                      for each control signal.
-**/
-typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum,
-                          long *gridChange);
-
-/**
- *  @brief  inv_set_grid_callback is used to register a callback function that
- *          will trigger when the grid location changes.
- *          inv_set_grid_callback allows a user to define a callback function that will
- *          run when a control signal crosses a grid threshold.
-
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().  inv_dmp_start must <b>NOT</b> have 
- *          been called.
- *
- *  @param  func    A user defined callback function
- *  @return Zero if the command is successful; an ML error code otherwise.
-**/
-inv_error_t inv_set_grid_callback(fpGridCb func)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    cntrl_params.gridCallback = func;
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  inv_set_control_data is used to assign physical parameters to control signals.
- *          inv_set_control_data allows flexibility in assigning physical parameters to
- *          control signals. For example, the user is allowed to use raw gyroscope data
- *          as an input to the control algorithm.
- *          Alternatively, angular velocity can be used, which combines gyroscopes and
- *          accelerometers to provide a more robust physical parameter. Finally, angular
- *          velocity in world coordinates can be used, providing a control signal in
- *          which pitch and yaw are provided relative to gravity.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param  controlSignal   Indicates which control signal is being modified.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 or
- *                          - INV_CONTROL_4.
- *
- *  @param  parameterArray   Indicates which parameter array is being assigned to a
- *                          control signal. Must be one of:
- *                          - INV_GYROS,
- *                          - INV_ANGULAR_VELOCITY, or
- *
- *  @param  parameterAxis   Indicates which axis of the parameter array will be used.
- *                          Must be:
- *                          - INV_ROLL,
- *                          - INV_PITCH, or
- *                          - INV_YAW.
- */
-
-inv_error_t inv_set_control_data(unsigned short controlSignal,
-                                 unsigned short parameterArray,
-                                 unsigned short parameterAxis)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char regs[2] = { DINA80 + 10, DINA20 };
-    inv_error_t result;
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    if (parameterArray == INV_ANGULAR_VELOCITY) {
-        regs[0] = DINA80 + 5;
-        regs[1] = DINA00;
-    }
-    switch (controlSignal) {
-    case INV_CONTROL_1:
-        cntrl_params.parameterArray[0] = parameterArray;
-        switch (parameterAxis) {
-        case INV_PITCH:
-            regs[1] += 0x02;
-            cntrl_params.parameterAxis[0] = 0;
-            break;
-        case INV_ROLL:
-            regs[1] = DINA22;
-            cntrl_params.parameterAxis[0] = 1;
-            break;
-        case INV_YAW:
-            regs[1] = DINA42;
-            cntrl_params.parameterAxis[0] = 2;
-            break;
-        default:
-            return INV_ERROR_INVALID_PARAMETER;
-        }
-        result = inv_set_mpu_memory(KEY_CFG_3, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case INV_CONTROL_2:
-        cntrl_params.parameterArray[1] = parameterArray;
-        switch (parameterAxis) {
-        case INV_PITCH:
-            regs[1] += DINA0E;
-            cntrl_params.parameterAxis[1] = 0;
-            break;
-        case INV_ROLL:
-            regs[1] += DINA2E;
-            cntrl_params.parameterAxis[1] = 1;
-            break;
-        case INV_YAW:
-            regs[1] += DINA4E;
-            cntrl_params.parameterAxis[1] = 2;
-            break;
-        default:
-            return INV_ERROR_INVALID_PARAMETER;
-        }
-        result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case INV_CONTROL_3:
-        cntrl_params.parameterArray[2] = parameterArray;
-        switch (parameterAxis) {
-        case INV_PITCH:
-            regs[1] += DINA0E;
-            cntrl_params.parameterAxis[2] = 0;
-            break;
-        case INV_ROLL:
-            regs[1] += DINA2E;
-            cntrl_params.parameterAxis[2] = 1;
-            break;
-        case INV_YAW:
-            regs[1] += DINA4E;
-            cntrl_params.parameterAxis[2] = 2;
-            break;
-        default:
-            return INV_ERROR_INVALID_PARAMETER;
-        }
-        result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case INV_CONTROL_4:
-        cntrl_params.parameterArray[3] = parameterArray;
-        switch (parameterAxis) {
-        case INV_PITCH:
-            regs[1] += DINA0E;
-            cntrl_params.parameterAxis[3] = 0;
-            break;
-        case INV_ROLL:
-            regs[1] += DINA2E;
-            cntrl_params.parameterAxis[3] = 1;
-            break;
-        case INV_YAW:
-            regs[1] += DINA4E;
-            cntrl_params.parameterAxis[3] = 2;
-            break;
-        default:
-            return INV_ERROR_INVALID_PARAMETER;
-        }
-        result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    default:
-        result = INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-    return result;
-}
-
-/**
- *  @brief  inv_get_control_data is used to get the current control data.
- *
- *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
- *          inv_open_low_power_pedometer().
- *
- *  @param  controlSignal   Indicates which control signal is being queried.
- *                          Must be one of:
- *                          - INV_CONTROL_1,
- *                          - INV_CONTROL_2,
- *                          - INV_CONTROL_3 or
- *                          - INV_CONTROL_4.
- *
- *  @param  gridNum     A pointer to pass gridNum info back to the user.
- *  @param  gridChange  A pointer to pass gridChange info back to the user.
- *
- *  @return Zero if the command is successful; an ML error code otherwise.
- */
-
-inv_error_t inv_get_control_data(long *controlSignal, long *gridNum,
-                                 long *gridChange)
-{
-    INVENSENSE_FUNC_START;
-    int_fast8_t i = 0;
-
-    if (inv_get_state() != INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    for (i = 0; i < 4; i++) {
-        controlSignal[i] = cntrl_obj.controlInt[i];
-        gridNum[i] = cntrl_obj.gridNum[i];
-        gridChange[i] = cntrl_obj.gridChange[i];
-    }
-    return INV_SUCCESS;
-}
-
-/** 
- * @internal
- * @brief   Update the ML Control engine.  This function should be called 
- *          every time new data from the MPU becomes available.
- *          Control engine outputs are written to the cntrl_obj data 
- *          structure.
- * @return  INV_SUCCESS or an error code.
-**/
-inv_error_t inv_update_control(struct inv_obj_t * inv_obj)
-{
-    INVENSENSE_FUNC_START;
-    unsigned char i;
-    long gridTmp;
-    long tmp;
-
-    inv_get_cntrl_data(cntrl_obj.mlGridNumDMP);
-
-    for (i = 0; i < 4; i++) {
-        if (cntrl_params.functions & INV_GRID) {
-            if (cntrl_params.functions & INV_HYSTERESIS) {
-                cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i];
-            }
-            cntrl_obj.mlGridNumDMP[i] =
-                cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L;
-            cntrl_obj.controlInt[i] =
-                (cntrl_obj.mlGridNumDMP[i] %
-                 (128 * cntrl_params.gridThreshold[i])) / 128;
-            gridTmp =
-                cntrl_obj.mlGridNumDMP[i] / (128 *
-                                             cntrl_params.gridThreshold[i]);
-            tmp = 1 + 16777216L / cntrl_params.gridThreshold[i];
-            cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i];
-            if (cntrl_obj.gridChange[i] > tmp / 2) {
-                cntrl_obj.gridChange[i] =
-                    gridTmp - tmp - cntrl_obj.lastGridNum[i];
-            } else if (cntrl_obj.gridChange[i] < -tmp / 2) {
-                cntrl_obj.gridChange[i] =
-                    gridTmp + tmp - cntrl_obj.lastGridNum[i];
-            }
-            if ((cntrl_params.functions & INV_HYSTERESIS)
-                && (cntrl_obj.gridChange[i] != 0)) {
-                if (cntrl_obj.gridChange[i] > 0) {
-                    cntrl_obj.gridNumOffset[i] +=
-                        128 * cntrl_params.gridThreshold[i];
-                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
-                }
-                if (cntrl_obj.gridChange[i] < 0) {
-                    cntrl_obj.gridNumOffset[i] -=
-                        128 * cntrl_params.gridThreshold[i];
-                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
-                }
-            }
-            cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i];
-            if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) {
-                cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i];
-                if (cntrl_obj.controlInt[i] >=
-                    cntrl_params.gridThreshold[i] / 2) {
-                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
-                }
-            } else if (cntrl_obj.gridNum[i] <= 0) {
-                cntrl_obj.gridNum[i] = 0;
-                if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) {
-                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
-                }
-            }
-            cntrl_obj.lastGridNum[i] = gridTmp;
-            if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) {
-                cntrl_params.gridCallback((INV_CONTROL_1 << i),
-                                          cntrl_obj.gridNum,
-                                          cntrl_obj.gridChange);
-            }
-
-        } else {
-            cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i];
-        }
-
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief Enables the INV_CONTROL engine.
- *
- * @note  This function replaces MLEnable(INV_CONTROL)
- *
- * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must 
- *      have been called.
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_enable_control(void)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() != INV_STATE_DMP_OPENED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    memset(&cntrl_obj, 0, sizeof(cntrl_obj));
-
-    inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL);   // fixme, someone needs to send control data to the fifo
-    return INV_SUCCESS;
-}
-
-/**
- * @brief Disables the INV_CONTROL engine.
- *
- * @note  This function replaces MLDisable(INV_CONTROL)
- *
- * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must 
- *      have been called.
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_disable_control(void)
-{
-    INVENSENSE_FUNC_START;
-
-    if (inv_get_state() < INV_STATE_DMP_STARTED)
-        return INV_ERROR_SM_IMPROPER_STATE;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/mlsdk/mllite/mlcontrol.h b/mlsdk/mllite/mlcontrol.h
deleted file mode 100644
index a834fc6..0000000
--- a/mlsdk/mllite/mlcontrol.h
+++ /dev/null
@@ -1,217 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $RCSfile: mlcontrol.h,v $
- *
- * $Date: 2011-06-10 20:13:08 -0700 (Fri, 10 Jun 2011) $
- *
- * $Revision: 5629 $
- *
- *******************************************************************************/
-
-/*******************************************************************************/
-/** @defgroup INV_CONTROL
-
-    The Control processes gyroscopes and accelerometers to provide control 
-    signals that can be used in user interfaces to manipulate objects such as 
-    documents, images, cursors, menus, etc.
-    
-    @{
-        @file mlcontrol.h
-        @brief Header file for the Control Library.
-*/
-/******************************************************************************/
-#ifndef MLCONTROL_H
-#define MLCONTROL_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "ml.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlcontrol_legacy.h"
-#endif
-
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-    /*******************************************************************************/
-    /* Control Signals.                                                            */
-    /*******************************************************************************/
-
-#define INV_CONTROL_1                    0x0001
-#define INV_CONTROL_2                    0x0002
-#define INV_CONTROL_3                    0x0004
-#define INV_CONTROL_4                    0x0008
-
-    /*******************************************************************************/
-    /* Control Functions.                                                          */
-    /*******************************************************************************/
-
-#define INV_GRID                         0x0001 // Indicates that the user will be controlling a system that
-    //   has discrete steps, such as icons, menu entries, pixels, etc.
-#define INV_SMOOTH                       0x0002 // Indicates that noise from unintentional motion should be filtered out.
-#define INV_DEAD_ZONE                    0x0004 // Indicates that a dead zone should be used, below which sensor data is set to zero.
-#define INV_HYSTERESIS                   0x0008 // Indicates that, when INV_GRID is selected, hysteresis should be used to prevent
-    //   the control signal from switching rapidly across elements of the grid.</dd>
-
-    /*******************************************************************************/
-    /* Integral reset options.                                                     */
-    /*******************************************************************************/
-
-#define INV_NO_RESET                     0x0000
-#define INV_RESET                        0x0001
-
-    /*******************************************************************************/
-    /* Data select options.                                                        */
-    /*******************************************************************************/
-
-#define INV_CTRL_SIGNAL                  0x0000
-#define INV_CTRL_GRID_NUM                0x0001
-
-    /*******************************************************************************/
-    /* Control Axis.                                                               */
-    /*******************************************************************************/
-#define INV_CTRL_PITCH                   0x0000 // (INV_PITCH >> 1)
-#define INV_CTRL_ROLL                    0x0001 // (INV_ROLL  >> 1)
-#define INV_CTRL_YAW                     0x0002 // (INV_YAW   >> 1)
-
-    /*******************************************************************************/
-    /* control_params structure default values.                                   */
-    /*******************************************************************************/
-
-#define MLCTRL_SENSITIVITY_0_DEFAULT           128
-#define MLCTRL_SENSITIVITY_1_DEFAULT           128
-#define MLCTRL_SENSITIVITY_2_DEFAULT           128
-#define MLCTRL_SENSITIVITY_3_DEFAULT           128
-#define MLCTRL_FUNCTIONS_DEFAULT                 0
-#define MLCTRL_CONTROL_SIGNALS_DEFAULT           0
-#define MLCTRL_PARAMETER_ARRAY_0_DEFAULT         0
-#define MLCTRL_PARAMETER_ARRAY_1_DEFAULT         0
-#define MLCTRL_PARAMETER_ARRAY_2_DEFAULT         0
-#define MLCTRL_PARAMETER_ARRAY_3_DEFAULT         0
-#define MLCTRL_PARAMETER_AXIS_0_DEFAULT          0
-#define MLCTRL_PARAMETER_AXIS_1_DEFAULT          0
-#define MLCTRL_PARAMETER_AXIS_2_DEFAULT          0
-#define MLCTRL_PARAMETER_AXIS_3_DEFAULT          0
-#define MLCTRL_GRID_THRESHOLD_0_DEFAULT          1
-#define MLCTRL_GRID_THRESHOLD_1_DEFAULT          1
-#define MLCTRL_GRID_THRESHOLD_2_DEFAULT          1
-#define MLCTRL_GRID_THRESHOLD_3_DEFAULT          1
-#define MLCTRL_GRID_MAXIMUM_0_DEFAULT            0
-#define MLCTRL_GRID_MAXIMUM_1_DEFAULT            0
-#define MLCTRL_GRID_MAXIMUM_2_DEFAULT            0
-#define MLCTRL_GRID_MAXIMUM_3_DEFAULT            0
-#define MLCTRL_GRID_CALLBACK_DEFAULT             0
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-    /**************************************************************************/
-    /* Control Parameters Structure.                                          */
-    /**************************************************************************/
-
-    struct control_params {
-        // Sensitivity of control signal 1, 2, 3, and 4.
-        unsigned short sensitivity[4];
-        // Indicates what functions will be used. Can be a bitwise OR of INV_GRID,
-        // ML_SMOOT, INV_DEAD_ZONE, and INV_HYSTERISIS.
-        unsigned short functions;
-        // Indicates which parameter array is being assigned to a control signal.
-        // Must be one of INV_GYROS, INV_ANGULAR_VELOCITY, or
-        // INV_ANGULAR_VELOCITY_WORLD.
-        unsigned short parameterArray[4];
-        // Indicates which axis of the parameter array will be used. Must be
-        // INV_ROLL, INV_PITCH, or INV_YAW.
-        unsigned short parameterAxis[4];
-        // Threshold of the control signal at which the grid number will be
-        // incremented or decremented.
-        long gridThreshold[4];
-        // Maximum grid number for the control signal.
-        long gridMaximum[4];
-        // User defined callback that will trigger when the grid location changes.
-        void (*gridCallback) (
-                                 // Indicates which control signal crossed a grid threshold. Must be
-                                 // one of INV_CONTROL_1, INV_CONTROL_2, INV_CONTROL_3 or INV_CONTROL_4.
-                                 unsigned short controlSignal,
-                                 // An array of four numbers representing the grid number for each
-                                 // control signal.
-                                 long *gridNum,
-                                 // An array of four numbers representing the change in grid number
-                                 // for each control signal.
-                                 long *gridChange);
-    };
-
-    struct control_obj {
-
-        long gridNum[4];        // Current grid number for each control signal.
-        long controlInt[4];     // Current data for each control signal.
-        long lastGridNum[4];    // Previous grid number
-        unsigned char controlDir[4];    // Direction of control signal
-        long gridChange[4];     // Change in grid number
-
-        long mlGridNumDMP[4];
-        long gridNumOffset[4];
-        long prevDMPGridNum[4];
-
-    };
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-
-    /**************************************************************************/
-    /* ML Control Functions.                                                  */
-    /**************************************************************************/
-
-    unsigned short inv_get_control_params(struct control_params *params);
-    unsigned short inv_set_control_params(struct control_params *params);
-
-    /*API for handling control signals */
-    inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
-                                            long sensitivity);
-    inv_error_t inv_set_control_func(unsigned short function);
-    inv_error_t inv_get_control_signal(unsigned short controlSignal,
-                                       unsigned short reset, long *data);
-    inv_error_t inv_get_grid_num(unsigned short controlSignal,
-                                 unsigned short reset, long *data);
-    inv_error_t inv_set_grid_thresh(unsigned short controlSignal,
-                                    long threshold);
-    inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum);
-    inv_error_t
-        inv_set_grid_callback(void (*func)
-                              (unsigned short controlSignal, long *gridNum,
-                               long *gridChange));
-    inv_error_t inv_set_control_data(unsigned short controlSignal,
-                                     unsigned short parameterArray,
-                                     unsigned short parameterNum);
-    inv_error_t inv_get_control_data(long *controlSignal, long *gridNum,
-                                     long *gridChange);
-    inv_error_t inv_update_control(struct inv_obj_t *inv_obj);
-    inv_error_t inv_enable_control(void);
-    inv_error_t inv_disable_control(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          /* MLCONTROL_H */
diff --git a/mlsdk/mllite/mldl.c b/mlsdk/mllite/mldl.c
deleted file mode 100644
index 7054532..0000000
--- a/mlsdk/mllite/mldl.c
+++ /dev/null
@@ -1,1092 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- *
- * $Id: mldl.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup   MLDL 
- *  @brief      Motion Library - Driver Layer.
- *              The Motion Library Driver Layer provides the intrface to the 
- *              system drivers that are used by the Motion Library.
- *
- *  @{
- *      @file   mldl.c
- *      @brief  The Motion Library Driver Layer.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-#    include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-#    include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-#  include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "compass.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlinclude.h"
-#include "ml.h"
-#include "dmpKey.h"
-#include "mlFIFOHW.h"
-#include "compass.h"
-#include "pressure.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mldl"
-
-#define _mldlDebug(x)           //{x}
-
-/* --------------------- */
-/* -    Variables.     - */
-/* --------------------- */
-
-#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2)   /* 128 */
-
-/*---- structure containing control variables used by MLDL ----*/
-static struct mldl_cfg mldlCfg;
-struct ext_slave_descr gAccel;
-struct ext_slave_descr gCompass;
-struct ext_slave_descr gPressure;
-struct mpu_platform_data gPdata;
-static void *sMLSLHandle;
-int_fast8_t intTrigger[NUM_OF_INTSOURCES];
-
-/*******************************************************************************
- * Functions for accessing the DMP memory via keys
- ******************************************************************************/
-
-unsigned short (*sGetAddress) (unsigned short key) = NULL;
-static const unsigned char *localDmpMemory = NULL;
-static unsigned short localDmpMemorySize = 0;
-
-/**
- *  @internal
- *  @brief Sets the function to use to convert keys to addresses. This
- *         will changed for each DMP code loaded.
- *  @param func
- *              Function used to convert keys to addresses.
- *  @endif
- */
-void inv_set_get_address(unsigned short (*func) (unsigned short key))
-{
-    INVENSENSE_FUNC_START;
-    _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func);
-        )
-        sGetAddress = func;
-}
-
-/**
- *  @internal
- *  @brief  Check if the feature is supported in the currently loaded
- *          DMP code basing on the fact that the key is assigned a
- *          value or not.
- *  @param  key     the DMP key
- *  @return whether the feature associated with the key is supported
- *          or not.
- */
-uint_fast8_t inv_dmpkey_supported(unsigned short key)
-{
-    unsigned short memAddr;
-
-    if (sGetAddress == NULL) {
-        MPL_LOGE("%s : sGetAddress is NULL\n", __func__);
-        return FALSE;
-    }
-
-    memAddr = sGetAddress(key);
-    if (memAddr >= 0xffff) {
-        MPL_LOGV("inv_set_mpu_memory unsupported key\n");
-        return FALSE;
-    }
-
-    return TRUE;
-}
-
-/**
- *  @internal
- *  @brief  used to get the specified number of bytes from the original
- *          MPU memory location specified by the key.
- *          Reads the specified number of bytes from the MPU location
- *          that was used to program the MPU specified by the key. Each
- *          set of code specifies a function that changes keys into
- *          addresses. This function is set with setGetAddress().
- *
- *  @param  key     The key to use when looking up the address.
- *  @param  length  Number of bytes to read.
- *  @param  buffer  Result for data.
- *
- *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- *          not corresponding to a memory address will result in INV_ERROR.
- *  @endif
- */
-inv_error_t inv_get_mpu_memory_original(unsigned short key,
-                                        unsigned short length,
-                                        unsigned char *buffer)
-{
-    unsigned short offset;
-
-    if (sGetAddress == NULL) {
-        return INV_ERROR_NOT_OPENED;
-    }
-
-    offset = sGetAddress(key);
-    if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    memcpy(buffer, &localDmpMemory[offset], length);
-
-    return INV_SUCCESS;
-}
-
-unsigned short inv_dl_get_address(unsigned short key)
-{
-    unsigned short offset;
-    if (sGetAddress == NULL) {
-        return INV_ERROR_NOT_OPENED;
-    }
-
-    offset = sGetAddress(key);
-    return offset;
-}
-
-/* ---------------------- */
-/* -  Static Functions. - */
-/* ---------------------- */
-
-/**
- *  @brief  Open the driver layer and resets the internal
- *          gyroscope, accelerometer, and compass data
- *          structures.
- *  @param  mlslHandle
- *              the serial handle.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_dl_open(void *mlslHandle)
-{
-    inv_error_t result;
-    memset(&mldlCfg, 0, sizeof(mldlCfg));
-    memset(intTrigger, INT_CLEAR, sizeof(intTrigger));
-
-    sMLSLHandle = mlslHandle;
-
-    mldlCfg.addr  = 0x68; /* default incase the driver doesn't set it */
-    mldlCfg.accel = &gAccel;
-    mldlCfg.compass = &gCompass;
-    mldlCfg.pressure = &gPressure;
-    mldlCfg.pdata = &gPdata;
-
-    result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle,
-                                        sMLSLHandle, sMLSLHandle, sMLSLHandle);
-    return result;
-}
-
-/**
- *  @brief  Closes/Cleans up the ML Driver Layer.
- *          Put the device in sleep mode.
- *  @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_dl_close(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    result = (inv_error_t) inv_mpu_suspend(&mldlCfg,
-                                           sMLSLHandle,
-                                           sMLSLHandle,
-                                           sMLSLHandle,
-                                           sMLSLHandle, 
-                                           INV_ALL_SENSORS);
-
-    result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle,
-                                         sMLSLHandle, sMLSLHandle, sMLSLHandle);
-    /* Clear all previous settings */
-    memset(&mldlCfg, 0, sizeof(mldlCfg));
-    sMLSLHandle = NULL;
-    sGetAddress = NULL;
-    return result;
-}
-
-/**
- * @brief Sets the requested_sensors
- *
- * Accessor to set the requested_sensors field of the mldl_cfg structure.
- * Typically set at initialization.
- *
- * @param sensors
- * Bitfield of the sensors that are going to be used.  Combination of the
- * following:
- *  - INV_X_GYRO
- *  - INV_Y_GYRO
- *  - INV_Z_GYRO
- *  - INV_DMP_PROCESSOR
- *  - INV_X_ACCEL
- *  - INV_Y_ACCEL
- *  - INV_Z_ACCEL
- *  - INV_X_COMPASS
- *  - INV_Y_COMPASS
- *  - INV_Z_COMPASS
- *  - INV_X_PRESSURE
- *  - INV_Y_PRESSURE
- *  - INV_Z_PRESSURE
- *  - INV_THREE_AXIS_GYRO
- *  - INV_THREE_AXIS_ACCEL
- *  - INV_THREE_AXIS_COMPASS
- *  - INV_THREE_AXIS_PRESSURE
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_init_requested_sensors(unsigned long sensors)
-{
-    mldlCfg.requested_sensors = sensors;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the DMP running
- *
- * Resumes the sensor if any of the sensor axis or components are requested
- *
- * @param sensors
- * Bitfield of the sensors to turn on.  Combination of the following:
- *  - INV_X_GYRO
- *  - INV_Y_GYRO
- *  - INV_Z_GYRO
- *  - INV_DMP_PROCESSOR
- *  - INV_X_ACCEL
- *  - INV_Y_ACCEL
- *  - INV_Z_ACCEL
- *  - INV_X_COMPASS
- *  - INV_Y_COMPASS
- *  - INV_Z_COMPASS
- *  - INV_X_PRESSURE
- *  - INV_Y_PRESSURE
- *  - INV_Z_PRESSURE
- *  - INV_THREE_AXIS_GYRO
- *  - INV_THREE_AXIS_ACCEL
- *  - INV_THREE_AXIS_COMPASS
- *  - INV_THREE_AXIS_PRESSURE
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_dl_start(unsigned long sensors)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    mldlCfg.requested_sensors = sensors;
-    result = inv_mpu_resume(&mldlCfg,
-                            sMLSLHandle,
-                            sMLSLHandle,
-                            sMLSLHandle,
-                            sMLSLHandle,
-                            sensors);
-    return result;
-}
-
-/**
- * @brief Stops the DMP running and puts it in low power as requested
- *
- * Suspends each sensor according to the bitfield, if all axis and components
- * of the sensor is off.
- *
- * @param sensors Bitfiled of the sensors to leave on.  Combination of the
- * following:
- *  - INV_X_GYRO
- *  - INV_Y_GYRO
- *  - INV_Z_GYRO
- *  - INV_X_ACCEL
- *  - INV_Y_ACCEL
- *  - INV_Z_ACCEL
- *  - INV_X_COMPASS
- *  - INV_Y_COMPASS
- *  - INV_Z_COMPASS
- *  - INV_X_PRESSURE
- *  - INV_Y_PRESSURE
- *  - INV_Z_PRESSURE
- *  - INV_THREE_AXIS_GYRO
- *  - INV_THREE_AXIS_ACCEL
- *  - INV_THREE_AXIS_COMPASS
- *  - INV_THREE_AXIS_PRESSURE
- *
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_dl_stop(unsigned long sensors)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result = INV_SUCCESS;
-
-    result = inv_mpu_suspend(&mldlCfg,
-                             sMLSLHandle,
-                             sMLSLHandle,
-                             sMLSLHandle,
-                             sMLSLHandle,
-                             sensors);
-    return result;
-}
-
-/**
- *  @brief  Get a pointer to the internal data structure
- *          storing the configuration for the MPU, the accelerometer
- *          and the compass in use.
- *  @return a pointer to the data structure of type 'struct mldl_cfg'.
- */
-struct mldl_cfg *inv_get_dl_config(void)
-{
-    return &mldlCfg;
-}
-
-/**
- *  @brief   Query the MPU slave address.
- *  @return  The 7-bit mpu slave address.
- */
-unsigned char inv_get_mpu_slave_addr(void)
-{
-    INVENSENSE_FUNC_START;
-    return mldlCfg.addr;
-}
-
-/**
- *  @internal
- * @brief   MLDLCfgDMP configures the Digital Motion Processor internal to
- *          the MPU. The DMP can be enabled or disabled and the start address
- *          can be set.
- *
- * @param   enableRun   Enables the DMP processing if set to TRUE.
- * @param   enableFIFO  Enables DMP output to the FIFO if set to TRUE.
- * @param   startAddress start address
- *
- * @return  Zero if the command is successful, an error code otherwise.
-*/
-inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
-                                unsigned char enableFIFO)
-{
-    INVENSENSE_FUNC_START;
-
-    mldlCfg.dmp_enable = enableRun;
-    mldlCfg.fifo_enable = enableFIFO;
-    mldlCfg.gyro_needs_reset = TRUE;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief   inv_get_dl_cfg_int configures the interrupt function on the specified pin.
- *          The basic interrupt signal characteristics can be set
- *          (i.e. active high/low, open drain/push pull, etc.) and the
- *          triggers can be set.
- *          Currently only INTPIN_MPU is supported.
- *
- * @param   triggers
- *              bitmask of triggers to enable for interrupt.
- *              The available triggers are:
- *              - BIT_MPU_RDY_EN
- *              - BIT_DMP_INT_EN
- *              - BIT_RAW_RDY_EN
- *
- * @return  Zero if the command is successful, an error code otherwise.
-*/
-inv_error_t inv_get_dl_cfg_int(unsigned char triggers)
-{
-    inv_error_t result = INV_SUCCESS;
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
-    /* Mantis has 8 bits of interrupt config bits */
-    if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-#endif
-
-    mldlCfg.int_config = triggers;
-    if (!mldlCfg.gyro_is_suspended) {
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_INT_CFG,
-                                         (mldlCfg.int_config | mldlCfg.pdata->
-                                          int_config));
-    } else {
-        mldlCfg.gyro_needs_reset = TRUE;
-    }
-
-    return result;
-}
-
-/**
- * @brief   configures the output sampling rate on the MPU.
- *          Three parameters control the sampling:
- *
- *          1) Low pass filter bandwidth, and
- *          2) output sampling divider.
- *
- *          The output sampling rate is determined by the divider and the low
- *          pass filter setting. If the low pass filter is set to
- *          'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the
- *          divider is 8kHz; for all other settings it is 1kHz.
- *          The 8-bit divider will divide this frequency to get the resulting
- *          sample frequency.
- *          For example, if the filter setting is not 256Hz and the divider is
- *          set to 7, then the sample rate is as follows:
- *          sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms).
- *
- *          The low pass filter selection codes control both the cutoff frequency of
- *          the internal low pass filter and internal analog sampling rate. The
- *          latter, in turn, affects the final output sampling rate according to the
- *          sample rate divider settig.
- *              0 -> 256 Hz  cutoff BW, 8 kHz analog sample rate,
- *              1 -> 188 Hz  cutoff BW, 1 kHz analog sample rate,
- *              2 ->  98 Hz  cutoff BW, 1 kHz analog sample rate,
- *              3 ->  42 Hz  cutoff BW, 1 kHz analog sample rate,
- *              4 ->  20 Hz  cutoff BW, 1 kHz analog sample rate,
- *              5 ->  10 Hz  cutoff BW, 1 kHz analog sample rate,
- *              6 ->   5 Hz  cutoff BW, 1 kHz analog sample rate,
- *              7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate.
- *
- * @param   lpf         low pass filter,   0 to 7.
- * @param   divider     Output sampling rate divider, 0 to 255.
- *
- * @return  ML_SUCESS if successful; a non-zero error code otherwise.
-**/
-inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider)
-{
-    /*---- do range checking ----*/
-    if (lpf >= NUM_MPU_FILTER) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    mldlCfg.lpf = lpf;
-    mldlCfg.divider = divider;
-    mldlCfg.gyro_needs_reset = TRUE;
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  set the full scale range for the gyros.
- *          The full scale selection codes correspond to:
- *              0 -> 250  dps,
- *              1 -> 500  dps,
- *              2 -> 1000 dps,
- *              3 -> 2000 dps.
- *          Full scale range affect the MPU's measurement
- *          sensitivity.
- *
- *  @param  fullScale
- *              the gyro full scale range in dps.
- *
- *  @return INV_SUCCESS or non-zero error code.
-**/
-inv_error_t inv_set_full_scale(float fullScale)
-{
-    if (fullScale == 250.f)
-        mldlCfg.full_scale = MPU_FS_250DPS;
-    else if (fullScale == 500.f)
-        mldlCfg.full_scale = MPU_FS_500DPS;
-    else if (fullScale == 1000.f)
-        mldlCfg.full_scale = MPU_FS_1000DPS;
-    else if (fullScale == 2000.f)
-        mldlCfg.full_scale = MPU_FS_2000DPS;
-    else {                      // not a valid setting
-        MPL_LOGE("Invalid full scale range specification for gyros : %f\n",
-                 fullScale);
-        MPL_LOGE
-            ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n");
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    mldlCfg.gyro_needs_reset = TRUE;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief   This function sets the external sync for the MPU sampling.
- *          It can be synchronized on the LSB of any of the gyros, any of the
- *          external accels, or on the temp readings.
- *
- * @param   extSync External sync selection, 0 to 7.
- * @return  Zero if the command is successful; an error code otherwise.
-**/
-inv_error_t inv_set_external_sync(unsigned char extSync)
-{
-    INVENSENSE_FUNC_START;
-
-    /*---- do range checking ----*/
-    if (extSync >= NUM_MPU_EXT_SYNC) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    mldlCfg.ext_sync = extSync;
-    mldlCfg.gyro_needs_reset = TRUE;
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_set_ignore_system_suspend(unsigned char ignore)
-{
-    INVENSENSE_FUNC_START;
-
-    mldlCfg.ignore_system_suspend = ignore;
-
-    return INV_SUCCESS;
-}
-
-/**
- * @brief   inv_clock_source function sets the clock source for the MPU gyro
- *          processing.
- *          The source can be any of the following:
- *          - Internal 8MHz oscillator,
- *          - PLL with X gyro as reference,
- *          - PLL with Y gyro as reference,
- *          - PLL with Z gyro as reference,
- *          - PLL with external 32.768Mhz reference, or
- *          - PLL with external 19.2MHz reference
- *
- *          For best accuracy and timing, it is highly recommended to use one
- *          of the gyros as the clock source; however this gyro must be
- *          enabled to use its clock (see 'MLDLPowerMgmtMPU()').
- *
- * @param   clkSource   Clock source selection.
- *                      Can be one of:
- *                      - CLK_INTERNAL,
- *                      - CLK_PLLGYROX,
- *                      - CLK_PLLGYROY,
- *                      - CLK_PLLGYROZ,
- *                      - CLK_PLLEXT32K, or
- *                      - CLK_PLLEXT19M.
- *
- * @return  Zero if the command is successful; an error code otherwise.
-**/
-inv_error_t inv_clock_source(unsigned char clkSource)
-{
-    INVENSENSE_FUNC_START;
-
-    /*---- do range checking ----*/
-    if (clkSource >= NUM_CLK_SEL) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    mldlCfg.clk_src = clkSource;
-    mldlCfg.gyro_needs_reset = TRUE;
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Set the Temperature Compensation offset.
- *  @param  tc
- *              a pointer to the temperature compensations offset
- *              for the 3 gyro axes.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_set_offsetTC(const unsigned char *tc)
-{
-    int ii;
-    inv_error_t result;
-
-    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) {
-        mldlCfg.offset_tc[ii] = tc[ii];
-    }
-
-    if (!mldlCfg.gyro_is_suspended) {
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_XG_OFFS_TC, tc[0]);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_YG_OFFS_TC, tc[1]);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_ZG_OFFS_TC, tc[2]);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-#else
-        unsigned char reg;
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_XG_OFFS_TC,
-                                         ((mldlCfg.offset_tc[0] << 1)
-                                          & BITS_XG_OFFS_TC));
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC);
-#ifdef CONFIG_MPU_SENSORS_MPU6050B1
-        if (mldlCfg.pdata->level_shifter)
-            reg |= BIT_I2C_MST_VDDIO;
-#endif
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_YG_OFFS_TC, reg);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
-                                         MPUREG_ZG_OFFS_TC,
-                                         ((mldlCfg.offset_tc[2] << 1)
-                                          & BITS_ZG_OFFS_TC));
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-#endif
-    } else {
-        mldlCfg.gyro_needs_reset = TRUE;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Set the gyro offset.
- *  @param  offset
- *              a pointer to the gyro offset for the 3 gyro axes. This is scaled
- *              as it would be written to the hardware registers.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_set_offset(const short *offset)
-{
-    inv_error_t result;
-    unsigned char regs[7];
-    int ii;
-    long sf;
-
-    sf = (2000L * 131) / mldlCfg.gyro_sens_trim;
-    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) {
-        // Record the bias in the units the register uses
-        mldlCfg.offset[ii] = offset[ii];
-        // Convert the bias to 1 dps = 1<<16
-        inv_obj.gyro_bias[ii] = -offset[ii] * sf;
-        regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff;
-        regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff);
-    }
-
-    if (!mldlCfg.gyro_is_suspended) {
-        regs[0] = MPUREG_X_OFFS_USRH;
-        result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    } else {
-        mldlCfg.gyro_needs_reset = TRUE;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @internal
- *  @brief  used to get the specified number of bytes in the specified MPU
- *          memory bank.
- *          The memory bank is one of the following:
- *          - MPUMEM_RAM_BANK_0,
- *          - MPUMEM_RAM_BANK_1,
- *          - MPUMEM_RAM_BANK_2, or
- *          - MPUMEM_RAM_BANK_3.
- *
- *  @param  bank    Memory bank to write.
- *  @param  memAddr Starting address for write.
- *  @param  length  Number of bytes to write.
- *  @param  buffer  Result for data.
- *
- *  @return zero if the command is successful, an error code otherwise.
- *  @endif
- */
-inv_error_t
-inv_get_mpu_memory_one_bank(unsigned char bank,
-                            unsigned char memAddr,
-                            unsigned short length, unsigned char *buffer)
-{
-    inv_error_t result;
-
-    if ((bank >= MPU_MEM_NUM_RAM_BANKS) ||
-        //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255
-        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    if (mldlCfg.gyro_is_suspended) {
-        memcpy(buffer, &mldlCfg.ram[bank][memAddr], length);
-        result = INV_SUCCESS;
-    } else {
-        result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr,
-                                     ((bank << 8) | memAddr), length, buffer);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  used to set the specified number of bytes in the specified MPU
- *          memory bank.
- *          The memory bank is one of the following:
- *          - MPUMEM_RAM_BANK_0,
- *          - MPUMEM_RAM_BANK_1,
- *          - MPUMEM_RAM_BANK_2, or
- *          - MPUMEM_RAM_BANK_3.
- *
- *  @param  bank    Memory bank to write.
- *  @param  memAddr Starting address for write.
- *  @param  length  Number of bytes to write.
- *  @param  buffer  Result for data.
- *
- *  @return zero if the command is successful, an error code otherwise.
- *  @endif
- */
-inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank,
-                                        unsigned short memAddr,
-                                        unsigned short length,
-                                        const unsigned char *buffer)
-{
-    inv_error_t result = INV_SUCCESS;
-    int different;
-
-    if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) ||
-        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length);
-    memcpy(&mldlCfg.ram[bank][memAddr], buffer, length);
-    if (!mldlCfg.gyro_is_suspended) {
-        result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr,
-                                      ((bank << 8) | memAddr), length, buffer);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    } else if (different) {
-        mldlCfg.gyro_needs_reset = TRUE;
-    }
-
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  used to get the specified number of bytes from the MPU location
- *          specified by the key.
- *          Reads the specified number of bytes from the MPU location
- *          specified by the key. Each set of code specifies a function
- *          that changes keys into addresses. This function is set with
- *          setGetAddress().
- *
- *  @param  key     The key to use when looking up the address.
- *  @param  length  Number of bytes to read.
- *  @param  buffer  Result for data.
- *
- *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- *          not corresponding to a memory address will result in INV_ERROR.
- *  @endif
- */
-inv_error_t inv_get_mpu_memory(unsigned short key,
-                               unsigned short length, unsigned char *buffer)
-{
-    unsigned char bank;
-    inv_error_t result;
-    unsigned short memAddr;
-
-    if (sGetAddress == NULL) {
-        return INV_ERROR_NOT_OPENED;
-    }
-
-    memAddr = sGetAddress(key);
-    if (memAddr >= 0xffff)
-        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-    bank = memAddr >> 8;        // Get Bank
-    memAddr &= 0xff;
-
-    while (memAddr + length > MPU_MEM_BANK_SIZE) {
-        // We cross a bank in the middle
-        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
-        result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
-                                             sub_length, buffer);
-        if (INV_SUCCESS != result)
-            return result;
-        bank++;
-        length -= sub_length;
-        buffer += sub_length;
-        memAddr = 0;
-    }
-    result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
-                                         length, buffer);
-
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  used to set the specified number of bytes from the MPU location
- *          specified by the key.
- *          Set the specified number of bytes from the MPU location
- *          specified by the key. Each set of DMP code specifies a function
- *          that changes keys into addresses. This function is set with
- *          setGetAddress().
- *
- *  @param  key     The key to use when looking up the address.
- *  @param  length  Number of bytes to write.
- *  @param  buffer  Result for data.
- *
- *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- *          not corresponding to a memory address will result in INV_ERROR.
- *  @endif
- */
-inv_error_t inv_set_mpu_memory(unsigned short key,
-                               unsigned short length,
-                               const unsigned char *buffer)
-{
-    inv_error_t result = INV_SUCCESS;
-    unsigned short memAddr;
-    unsigned char bank;
-
-    if (sGetAddress == NULL) {
-        MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n");
-        return INV_ERROR_INVALID_MODULE;
-    }
-    memAddr = sGetAddress(key);
-
-    if (memAddr >= 0xffff) {
-        MPL_LOGE("inv_set_mpu_memory unsupported key\n");
-        return INV_ERROR_INVALID_MODULE; // This key not supported
-    }
-
-    bank = (unsigned char)(memAddr >> 8);
-    memAddr &= 0xff;
-
-    while (memAddr + length > MPU_MEM_BANK_SIZE) {
-        // We cross a bank in the middle
-        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
-
-        result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        bank++;
-        length -= sub_length;
-        buffer += sub_length;
-        memAddr = 0;
-    }
-    result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/**
- *  @brief  Load the DMP with the given code and configuration.
- *  @param  buffer
- *              the DMP data.
- *  @param  length
- *              the length in bytes of the DMP data.
- *  @param  config
- *              the DMP configuration.
- *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_dmp(const unsigned char *buffer,
-                         unsigned short length, unsigned short config)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result = INV_SUCCESS;
-    unsigned short toWrite;
-    unsigned short memAddr = 0;
-    localDmpMemory = buffer;
-    localDmpMemorySize = length;
-
-    mldlCfg.dmp_cfg1 = (config >> 8);
-    mldlCfg.dmp_cfg2 = (config & 0xff);
-
-    while (length > 0) {
-        toWrite = length;
-        if (toWrite > MAX_LOAD_WRITE_SIZE)
-            toWrite = MAX_LOAD_WRITE_SIZE;
-
-        result =
-            inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite,
-                                        buffer);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        buffer += toWrite;
-        memAddr += toWrite;
-        length -= toWrite;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  Get the silicon revision ID.
- *  @return The silicon revision ID
- *          (0 will be read if inv_mpu_open returned an error)
- */
-unsigned char inv_get_silicon_rev(void)
-{
-    return mldlCfg.silicon_revision;
-}
-
-/**
- *  @brief  Get the product revision ID.
- *  @return The product revision ID
- *          (0 will be read if inv_mpu_open returned an error)
- */
-unsigned char inv_get_product_rev(void)
-{
-    return mldlCfg.product_revision;
-}
-
-/*******************************************************************************
- *******************************************************************************
- *******************************************************************************
- * @todo these belong with an interface to the kernel driver layer
- *******************************************************************************
- *******************************************************************************
- ******************************************************************************/
-
-/**
- * @brief   inv_get_interrupt_status returns the interrupt status from the specified
- *          interrupt pin.
- * @param   intPin
- *              Currently only the value INTPIN_MPU is supported.
- * @param   status
- *              The available statuses are:
- *              - BIT_MPU_RDY_EN
- *              - BIT_DMP_INT_EN
- *              - BIT_RAW_RDY_EN
- *
- * @return  INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_interrupt_status(unsigned char intPin,
-                                     unsigned char *status)
-{
-    INVENSENSE_FUNC_START;
-
-    inv_error_t result;
-
-    switch (intPin) {
-
-    case INTPIN_MPU:
-            /*---- return the MPU interrupt status ----*/
-        result = inv_serial_read(sMLSLHandle, mldlCfg.addr,
-                                 MPUREG_INT_STATUS, 1, status);
-        break;
-
-    default:
-        result = INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return result;
-}
-
-/**
- *  @brief   query the current status of an interrupt source.
- *  @param   srcIndex
- *              index of the interrupt source.
- *              Currently the only source supported is INTPIN_MPU.
- *
- *  @return  1 if the interrupt has been triggered.
- */
-unsigned char inv_get_interrupt_trigger(unsigned char srcIndex)
-{
-    INVENSENSE_FUNC_START;
-    return intTrigger[srcIndex];
-}
-
-/**
- * @brief clear the 'triggered' status for an interrupt source.
- * @param srcIndex
- *          index of the interrupt source.
- *          Currently only INTPIN_MPU is supported.
- */
-void inv_clear_interrupt_trigger(unsigned char srcIndex)
-{
-    INVENSENSE_FUNC_START;
-    intTrigger[srcIndex] = 0;
-}
-
-/**
- * @brief   inv_interrupt_handler function should be called when an interrupt is
- *          received.  The source parameter identifies which interrupt source
- *          caused the interrupt. Note that this routine should not be called
- *          directly from the interrupt service routine.
- *
- * @param   intSource   MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1,
- *                      INTSRC_AUX2, or INT_SRC_TIMER.
- *
- * @return  Zero if the command is successful; an error code otherwise.
- */
-inv_error_t inv_interrupt_handler(unsigned char intSource)
-{
-    INVENSENSE_FUNC_START;
-    /*---- range check ----*/
-    if (intSource >= NUM_OF_INTSOURCES) {
-        return INV_ERROR;
-    }
-
-    /*---- save source of interrupt ----*/
-    intTrigger[intSource] = INT_TRIGGERED;
-
-#ifdef ML_USE_DMP_SIM
-    if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) {
-        MLSimHWDataInput();
-    }
-#endif
-
-    return INV_SUCCESS;
-}
-
-/***************************/
-        /**@}*//* end of defgroup */
-/***************************/
diff --git a/mlsdk/mllite/mldl.h b/mlsdk/mllite/mldl.h
deleted file mode 100644
index 961d860..0000000
--- a/mlsdk/mllite/mldl.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: mldl.h 5639 2011-06-14 01:23:05Z nroyer $
- *
- *******************************************************************************/
-
-#ifndef MLDL_H
-#define MLDL_H
-
-#include "mltypes.h"
-#include "mlsl.h"
-#include <linux/mpu.h>
-#include "mldl_cfg.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mldl_legacy.h"
-#endif
-
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-typedef enum _DEVICE_CONFIG {
-    DEVICE_MPU_ACCEL = 0,
-    DEVICE_MPU,
-    NUM_OF_DEVICES
-} DEVICE_CONFIG;
-
-#define SERIAL_I2C                  0
-#define SERIAL_SPI                  1
-
-#define DATAMODE_MANUAL             0   // Manual data mode
-#define DATAMODE_AUTO               1   // Auto data mode
-
-#define DATASRC_IMMEDIATE           0   // Return data immediately
-#define DATASRC_WHENREADY           1   // Only return data when new data is available
-#define DATASRC_FIFO                2   // Use FIFO for data
-
-#define SENSOR_DATA_GYROX           0x0001
-#define SENSOR_DATA_GYROY           0x0002
-#define SENSOR_DATA_GYROZ           0x0004
-#define SENSOR_DATA_ACCELX          0x0008
-#define SENSOR_DATA_ACCELY          0x0010
-#define SENSOR_DATA_ACCELZ          0x0020
-#define SENSOR_DATA_AUX1            0x0040
-#define SENSOR_DATA_AUX2            0x0080
-#define SENSOR_DATA_AUX3            0x0100
-#define SENSOR_DATA_TEMP            0x0200
-
-#define INTPIN_MPU                  0
-
-#define INTLOGIC_HIGH               0
-#define INTLOGIC_LOW                1
-
-#define INTTYPE_PUSHPULL            0
-#define INTTYPE_OPENDRAIN           1
-
-#define INTLATCH_DISABLE            0
-#define INTLATCH_ENABLE             1
-
-#define MPUINT_MPU_READY            0x04
-#define MPUINT_DMP_DONE             0x02
-#define MPUINT_DATA_READY           0x01
-
-#define INTLATCHCLEAR_READSTATUS    0
-#define INTLATCHCLEAR_ANYREAD       1
-
-#define DMP_DONTRUN                 0
-#define DMP_RUN                     1
-
-    /*---- defines for external interrupt status (via external call into library) ----*/
-#define INT_CLEAR                   0
-#define INT_TRIGGERED               1
-
-typedef enum {
-    INTSRC_MPU = 0,
-    INTSRC_AUX1,
-    INTSRC_AUX2,
-    INTSRC_AUX3,
-    INTSRC_TIMER,
-    INTSRC_UNKNOWN,
-    INTSRC_MPU_FIFO,
-    INTSRC_MPU_MOTION,
-    NUM_OF_INTSOURCES,
-} INT_SOURCE;
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-    /* --------------- */
-    /* - Variables.  - */
-    /* --------------- */
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    inv_error_t inv_dl_open(void *mlslHandle);
-    inv_error_t inv_dl_close(void);
-
-    inv_error_t inv_dl_start(unsigned long sensors);
-    inv_error_t inv_dl_stop(unsigned long sensors);
-
-    struct mldl_cfg *inv_get_dl_config(void);
-
-    inv_error_t inv_init_requested_sensors(unsigned long sensors);
-    unsigned char inv_get_mpu_slave_addr(void);
-    inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
-                                    unsigned char enableFIFO);
-    inv_error_t inv_get_dl_cfg_int(unsigned char triggers);
-    inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider);
-    inv_error_t inv_set_full_scale(float fullScale);
-    inv_error_t inv_set_external_sync(unsigned char extSync);
-    inv_error_t inv_set_ignore_system_suspend(unsigned char ignore);
-    inv_error_t inv_clock_source(unsigned char clkSource);
-    inv_error_t inv_get_mpu_memory(unsigned short key,
-                                   unsigned short length,
-                                   unsigned char *buffer);
-    inv_error_t inv_set_mpu_memory(unsigned short key,
-                                   unsigned short length,
-                                   const unsigned char *buffer);
-    inv_error_t inv_load_dmp(const unsigned char *buffer,
-                             unsigned short length,
-                             unsigned short startAddress);
-
-    unsigned char inv_get_slicon_rev(void);
-    inv_error_t inv_set_offsetTC(const unsigned char *tc);
-    inv_error_t inv_set_offset(const short *offset);
-
-    /* Functions for setting and retrieving the DMP memory */
-    inv_error_t inv_get_mpu_memory_original(unsigned short key,
-                                            unsigned short length,
-                                            unsigned char *buffer);
-    void inv_set_get_address(unsigned short (*func) (unsigned short key));
-    unsigned short inv_dl_get_address(unsigned short key);
-    uint_fast8_t inv_dmpkey_supported(unsigned short key);
-
-    inv_error_t inv_get_interrupt_status(unsigned char intPin,
-                                         unsigned char *value);
-    unsigned char inv_get_interrupt_trigger(unsigned char index);
-    void inv_clear_interrupt_trigger(unsigned char index);
-    inv_error_t inv_interrupt_handler(unsigned char intSource);
-
-    /** Only exposed for testing purposes */
-    inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank,
-                                            unsigned short memAddr,
-                                            unsigned short length,
-                                            const unsigned char *buffer);
-    inv_error_t inv_get_mpu_memory_one_bank(unsigned char bank,
-                                            unsigned char memAddr,
-                                            unsigned short length,
-                                            unsigned char *buffer);
-#ifdef __cplusplus
-}
-#endif
-#endif                          // MLDL_H
diff --git a/mlsdk/mllite/mldl_cfg.h b/mlsdk/mllite/mldl_cfg.h
deleted file mode 100644
index b4914e2..0000000
--- a/mlsdk/mllite/mldl_cfg.h
+++ /dev/null
@@ -1,336 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/**
- *  @addtogroup MLDL
- *
- *  @{
- *      @file   mldl_cfg.h
- *      @brief  The Motion Library Driver Layer Configuration header file.
- */
-
-#ifndef __MLDL_CFG_H__
-#define __MLDL_CFG_H__
-
-#include "mltypes.h"
-#include "mlsl.h"
-#include <linux/mpu.h>
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-#    include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-#    include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-#  include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-
-#include "log.h"
-
-/*************************************************************************
- *  Sensors
- *************************************************************************/
-
-#define INV_X_GYRO			(0x0001)
-#define INV_Y_GYRO			(0x0002)
-#define INV_Z_GYRO			(0x0004)
-#define INV_DMP_PROCESSOR		(0x0008)
-
-#define INV_X_ACCEL			(0x0010)
-#define INV_Y_ACCEL			(0x0020)
-#define INV_Z_ACCEL			(0x0040)
-
-#define INV_X_COMPASS			(0x0080)
-#define INV_Y_COMPASS			(0x0100)
-#define INV_Z_COMPASS			(0x0200)
-
-#define INV_X_PRESSURE			(0x0300)
-#define INV_Y_PRESSURE			(0x0800)
-#define INV_Z_PRESSURE			(0x1000)
-
-#define INV_TEMPERATURE			(0x2000)
-#define INV_TIME			(0x4000)
-
-#define INV_THREE_AXIS_GYRO		(0x000F)
-#define INV_THREE_AXIS_ACCEL		(0x0070)
-#define INV_THREE_AXIS_COMPASS		(0x0380)
-#define INV_THREE_AXIS_PRESSURE		(0x1C00)
-
-#define INV_FIVE_AXIS			(0x007B)
-#define INV_SIX_AXIS_GYRO_ACCEL		(0x007F)
-#define INV_SIX_AXIS_ACCEL_COMPASS	(0x03F0)
-#define INV_NINE_AXIS			(0x03FF)
-#define INV_ALL_SENSORS			(0x7FFF)
-
-#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
-
-/* -------------------------------------------------------------------------- */
-
-/* Platform data for the MPU */
-struct mldl_cfg {
-	/* MPU related configuration */
-	unsigned long requested_sensors;
-	unsigned char ignore_system_suspend;
-	unsigned char addr;
-	unsigned char int_config;
-	unsigned char ext_sync;
-	unsigned char full_scale;
-	unsigned char lpf;
-	unsigned char clk_src;
-	unsigned char divider;
-	unsigned char dmp_enable;
-	unsigned char fifo_enable;
-	unsigned char dmp_cfg1;
-	unsigned char dmp_cfg2;
-	unsigned char offset_tc[GYRO_NUM_AXES];
-	unsigned short offset[GYRO_NUM_AXES];
-	unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE];
-
-	/* MPU Related stored status and info */
-	unsigned char product_revision;
-	unsigned char silicon_revision;
-	unsigned char product_id;
-	unsigned short gyro_sens_trim;
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-	unsigned short accel_sens_trim;
-#endif
-
-	/* Driver/Kernel related state information */
-	int gyro_is_bypassed;
-	int i2c_slaves_enabled;
-	int dmp_is_running;
-	int gyro_is_suspended;
-	int accel_is_suspended;
-	int compass_is_suspended;
-	int pressure_is_suspended;
-	int gyro_needs_reset;
-
-	/* Slave related information */
-	struct ext_slave_descr *accel;
-	struct ext_slave_descr *compass;
-	struct ext_slave_descr *pressure;
-
-	/* Platform Data */
-	struct mpu_platform_data *pdata;
-};
-
-/* -------------------------------------------------------------------------- */
-
-int inv_mpu_open(struct mldl_cfg *mldl_cfg,
-		 void *mlsl_handle,
-		 void *accel_handle,
-		 void *compass_handle,
-		 void *pressure_handle);
-int inv_mpu_close(struct mldl_cfg *mldl_cfg,
-		  void *mlsl_handle,
-		  void *accel_handle,
-		  void *compass_handle,
-		  void *pressure_handle);
-int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
-		   void *gyro_handle,
-		   void *accel_handle,
-		   void *compass_handle,
-		   void *pressure_handle,
-		   unsigned long sensors);
-int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
-		    void *gyro_handle,
-		    void *accel_handle,
-		    void *compass_handle,
-		    void *pressure_handle,
-		    unsigned long sensors);
-
-/* Slave Read functions */
-int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
-		       void *gyro_handle,
-		       void *slave_handle,
-		       struct ext_slave_descr *slave,
-		       struct ext_slave_platform_data *pdata,
-		       unsigned char *data);
-static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg,
-				     void *gyro_handle,
-				     void *accel_handle, unsigned char *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_read(mldl_cfg, gyro_handle, accel_handle,
-				  mldl_cfg->accel, &mldl_cfg->pdata->accel,
-				  data);
-}
-
-static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg,
-				       void *gyro_handle,
-				       void *compass_handle,
-				       unsigned char *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_read(mldl_cfg, gyro_handle, compass_handle,
-				  mldl_cfg->compass, &mldl_cfg->pdata->compass,
-				  data);
-}
-
-static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg,
-					void *gyro_handle,
-					void *pressure_handle,
-					unsigned char *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_read(mldl_cfg, gyro_handle, pressure_handle,
-				  mldl_cfg->pressure,
-				  &mldl_cfg->pdata->pressure, data);
-}
-
-/* Slave Config functions */
-int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
-			 void *gyro_handle,
-			 void *slave_handle,
-			 struct ext_slave_config *data,
-			 struct ext_slave_descr *slave,
-			 struct ext_slave_platform_data *pdata);
-static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg,
-				       void *gyro_handle,
-				       void *accel_handle,
-				       struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_config(mldl_cfg, gyro_handle, accel_handle, data,
-				    mldl_cfg->accel, &mldl_cfg->pdata->accel);
-}
-
-static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg,
-					 void *gyro_handle,
-					 void *compass_handle,
-					 struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_config(mldl_cfg, gyro_handle, compass_handle, data,
-				    mldl_cfg->compass,
-				    &mldl_cfg->pdata->compass);
-}
-
-static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg,
-					  void *gyro_handle,
-					  void *pressure_handle,
-					  struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_slave_config(mldl_cfg, gyro_handle, pressure_handle,
-				    data, mldl_cfg->pressure,
-				    &mldl_cfg->pdata->pressure);
-}
-
-/* Slave get config functions */
-int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
-			     void *gyro_handle,
-			     void *slave_handle,
-			     struct ext_slave_config *data,
-			     struct ext_slave_descr *slave,
-			     struct ext_slave_platform_data *pdata);
-
-static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg,
-					   void *gyro_handle,
-					   void *accel_handle,
-					   struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, accel_handle,
-					data, mldl_cfg->accel,
-					&mldl_cfg->pdata->accel);
-}
-
-static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg,
-					     void *gyro_handle,
-					     void *compass_handle,
-					     struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, compass_handle,
-					data, mldl_cfg->compass,
-					&mldl_cfg->pdata->compass);
-}
-
-static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg,
-					      void *gyro_handle,
-					      void *pressure_handle,
-					      struct ext_slave_config *data)
-{
-	if (!mldl_cfg || !(mldl_cfg->pdata)) {
-		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-		return INV_ERROR_INVALID_PARAMETER;
-	}
-
-	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle,
-					pressure_handle, data,
-					mldl_cfg->pressure,
-					&mldl_cfg->pdata->pressure);
-}
-
-/* -------------------------------------------------------------------------- */
-
-static inline long inv_mpu_get_sampling_rate_hz(struct mldl_cfg *mldl_cfg)
-{
-	if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
-		return 8000L / (mldl_cfg->divider + 1);
-	else
-		return 1000L / (mldl_cfg->divider + 1);
-}
-
-static inline long inv_mpu_get_sampling_period_us(struct mldl_cfg *mldl_cfg)
-{
-	if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
-		return (long) (1000000L * (mldl_cfg->divider + 1)) / 8000L;
-	else
-		return (long) (1000000L * (mldl_cfg->divider + 1)) / 1000L;
-}
-
-#endif				/* __MLDL_CFG_H__ */
-
-/**
- *@}
- */
diff --git a/mlsdk/mllite/mldl_cfg_mpu.c b/mlsdk/mllite/mldl_cfg_mpu.c
deleted file mode 100644
index f89f94c..0000000
--- a/mlsdk/mllite/mldl_cfg_mpu.c
+++ /dev/null
@@ -1,477 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/** 
- *  @addtogroup MLDL
- *
- *  @{
- *      @file   mldl_cfg_mpu.c
- *      @brief  The Motion Library Driver Layer.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <stddef.h>
-#include "mldl_cfg.h"
-#include "mlsl.h"
-#include "mpu.h"
-
-#ifdef LINUX
-#include <sys/ioctl.h>
-#endif
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:"
-
-/* --------------------- */
-/* -    Variables.     - */
-/* --------------------- */
-
-
-/* ---------------------- */
-/* -  Static Functions. - */
-/* ---------------------- */
-void mpu_print_cfg(struct mldl_cfg * mldl_cfg)
-{
-    struct mpu_platform_data   *pdata   = mldl_cfg->pdata;
-    struct ext_slave_platform_data *accel   = &mldl_cfg->pdata->accel;
-    struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass;
-    struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure;
-
-    MPL_LOGD("mldl_cfg.addr             = %02x\n", mldl_cfg->addr);
-    MPL_LOGD("mldl_cfg.int_config       = %02x\n", mldl_cfg->int_config);
-    MPL_LOGD("mldl_cfg.ext_sync         = %02x\n", mldl_cfg->ext_sync);
-    MPL_LOGD("mldl_cfg.full_scale       = %02x\n", mldl_cfg->full_scale);
-    MPL_LOGD("mldl_cfg.lpf              = %02x\n", mldl_cfg->lpf);
-    MPL_LOGD("mldl_cfg.clk_src          = %02x\n", mldl_cfg->clk_src);
-    MPL_LOGD("mldl_cfg.divider          = %02x\n", mldl_cfg->divider);
-    MPL_LOGD("mldl_cfg.dmp_enable       = %02x\n", mldl_cfg->dmp_enable);
-    MPL_LOGD("mldl_cfg.fifo_enable      = %02x\n", mldl_cfg->fifo_enable);
-    MPL_LOGD("mldl_cfg.dmp_cfg1         = %02x\n", mldl_cfg->dmp_cfg1);
-    MPL_LOGD("mldl_cfg.dmp_cfg2         = %02x\n", mldl_cfg->dmp_cfg2);
-    MPL_LOGD("mldl_cfg.offset_tc[0]     = %02x\n", mldl_cfg->offset_tc[0]);
-    MPL_LOGD("mldl_cfg.offset_tc[1]     = %02x\n", mldl_cfg->offset_tc[1]);
-    MPL_LOGD("mldl_cfg.offset_tc[2]     = %02x\n", mldl_cfg->offset_tc[2]);
-    MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision);
-    MPL_LOGD("mldl_cfg.product_id       = %02x\n", mldl_cfg->product_id);
-    MPL_LOGD("mldl_cfg.gyro_sens_trim   = %02x\n", mldl_cfg->gyro_sens_trim);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
-    MPL_LOGD("mldl_cfg.accel_sens_trim   = %02x\n", mldl_cfg->accel_sens_trim);
-#endif
-
-    if (mldl_cfg->accel) {
-        MPL_LOGD("slave_accel->suspend      = %02x\n", (int)mldl_cfg->accel->suspend);
-        MPL_LOGD("slave_accel->resume       = %02x\n", (int)mldl_cfg->accel->resume);
-        MPL_LOGD("slave_accel->read         = %02x\n", (int)mldl_cfg->accel->read);
-        MPL_LOGD("slave_accel->type         = %02x\n", mldl_cfg->accel->type);
-        MPL_LOGD("slave_accel->read_reg     = %02x\n",
-                 mldl_cfg->accel->read_reg);
-        MPL_LOGD("slave_accel->read_len     = %02x\n",
-                 mldl_cfg->accel->read_len);
-        MPL_LOGD("slave_accel->endian       = %02x\n", mldl_cfg->accel->endian);
-        MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa);
-        MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction);
-    } else {
-        MPL_LOGD("slave_accel               = NULL\n");
-    }
-
-    if (mldl_cfg->compass) {
-        MPL_LOGD("slave_compass->suspend    = %02x\n", (int)mldl_cfg->compass->suspend);
-        MPL_LOGD("slave_compass->resume     = %02x\n", (int)mldl_cfg->compass->resume);
-        MPL_LOGD("slave_compass->read       = %02x\n", (int)mldl_cfg->compass->read);
-        MPL_LOGD("slave_compass->type       = %02x\n", mldl_cfg->compass->type);
-        MPL_LOGD("slave_compass->read_reg   = %02x\n",
-                 mldl_cfg->compass->read_reg);
-        MPL_LOGD("slave_compass->read_len   = %02x\n",
-                 mldl_cfg->compass->read_len);
-        MPL_LOGD("slave_compass->endian     = %02x\n", mldl_cfg->compass->endian);
-        MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa);
-        MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction);
-    } else {
-        MPL_LOGD("slave_compass             = NULL\n");
-    }
-
-    if (mldl_cfg->pressure) {
-        MPL_LOGD("slave_pressure->suspend    = %02x\n", (int)mldl_cfg->pressure->suspend);
-        MPL_LOGD("slave_pressure->resume     = %02x\n", (int)mldl_cfg->pressure->resume);
-        MPL_LOGD("slave_pressure->read       = %02x\n", (int)mldl_cfg->pressure->read);
-        MPL_LOGD("slave_pressure->type       = %02x\n", mldl_cfg->pressure->type);
-        MPL_LOGD("slave_pressure->read_reg   = %02x\n",
-                 mldl_cfg->pressure->read_reg);
-        MPL_LOGD("slave_pressure->read_len   = %02x\n",
-                 mldl_cfg->pressure->read_len);
-        MPL_LOGD("slave_pressure->endian     = %02x\n", mldl_cfg->pressure->endian);
-        MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa);
-        MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction);
-    } else {
-        MPL_LOGD("slave_pressure             = NULL\n");
-    }
-
-    MPL_LOGD("accel->get_slave_descr    = %x\n",(unsigned int) accel->get_slave_descr);
-    MPL_LOGD("accel->adapt_num          = %02x\n", accel->adapt_num);
-    MPL_LOGD("accel->bus                = %02x\n", accel->bus);
-    MPL_LOGD("accel->address            = %02x\n", accel->address);
-    MPL_LOGD("accel->orientation        = \n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n",
-             accel->orientation[0],accel->orientation[1],accel->orientation[2],
-             accel->orientation[3],accel->orientation[4],accel->orientation[5],
-             accel->orientation[6],accel->orientation[7],accel->orientation[8]);
-    MPL_LOGD("compass->get_slave_descr  = %x\n",(unsigned int) compass->get_slave_descr);
-    MPL_LOGD("compass->adapt_num        = %02x\n", compass->adapt_num);
-    MPL_LOGD("compass->bus              = %02x\n", compass->bus);
-    MPL_LOGD("compass->address          = %02x\n", compass->address);
-    MPL_LOGD("compass->orientation      = \n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n",
-             compass->orientation[0],compass->orientation[1],compass->orientation[2],
-             compass->orientation[3],compass->orientation[4],compass->orientation[5],
-             compass->orientation[6],compass->orientation[7],compass->orientation[8]);
-    MPL_LOGD("pressure->get_slave_descr  = %x\n",(unsigned int) pressure->get_slave_descr);
-    MPL_LOGD("pressure->adapt_num        = %02x\n", pressure->adapt_num);
-    MPL_LOGD("pressure->bus              = %02x\n", pressure->bus);
-    MPL_LOGD("pressure->address          = %02x\n", pressure->address);
-    MPL_LOGD("pressure->orientation      = \n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n",
-             pressure->orientation[0],pressure->orientation[1],pressure->orientation[2],
-             pressure->orientation[3],pressure->orientation[4],pressure->orientation[5],
-             pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]);
-    
-    MPL_LOGD("pdata->int_config         = %02x\n", pdata->int_config);
-    MPL_LOGD("pdata->level_shifter      = %02x\n", pdata->level_shifter);
-    MPL_LOGD("pdata->orientation        = \n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n"
-             "                            %2d %2d %2d\n",
-             pdata->orientation[0],pdata->orientation[1],pdata->orientation[2],
-             pdata->orientation[3],pdata->orientation[4],pdata->orientation[5],
-             pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]);
-
-    MPL_LOGD("Struct sizes: mldl_cfg: %d, "
-             "ext_slave_descr:%d, mpu_platform_data:%d: RamOffset: %d\n", 
-             sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr), 
-             sizeof(struct mpu_platform_data), 
-             offsetof(struct mldl_cfg, ram));
-}
-
-/******************************************************************************
- ******************************************************************************
- * Exported functions
- ******************************************************************************
- *****************************************************************************/
-
-/** 
- * Initializes the pdata structure to defaults.
- *
- * Opens the device to read silicon revision, product id and whoami.  Leaves
- * the device in suspended state for low power.
- * 
- * @param mldl_cfg handle to the config structure
- * @param mlsl_handle handle to the mpu serial layer
- * @param accel_handle handle to the accel serial layer
- * @param compass_handle handle to the compass serial layer
- * @param pressure_handle handle to the pressure serial layer
- *
- * @return INV_SUCCESS if silicon revision, product id and woami are supported
- *         by this software.
- */
-int inv_mpu_open(struct mldl_cfg *mldl_cfg, 
-                 void *mlsl_handle,
-                 void *accel_handle,
-                 void *compass_handle,
-                 void *pressure_handle)
-{
-    int result;
-    result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
-                             INV_ALL_SENSORS);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    return result;
-}
-
-/** 
- * Stub for driver close.  Just verify that the devices are suspended
- * 
- * @param mldl_cfg handle to the config structure
- * @param mlsl_handle handle to the mpu serial layer
- * @param accel_handle handle to the accel serial layer
- * @param compass_handle handle to the compass serial layer
- * @param pressure_handle handle to the compass serial layer
- * 
- * @return INV_SUCCESS or non-zero error code
- */
-int inv_mpu_close(struct mldl_cfg *mldl_cfg, 
-		  void *mlsl_handle,
-		  void *accel_handle,
-		  void *compass_handle,
-		  void *pressure_handle)
-{
-    int result = INV_SUCCESS;
-
-    result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
-                             INV_ALL_SENSORS);
-    return result;
-}
-
-int inv_mpu_resume(struct mldl_cfg* mldl_cfg, 
-                   void *mlsl_handle, 
-                   void *accel_handle, 
-                   void *compass_handle, 
-                   void *pressure_handle,
-                   unsigned long sensors)
-{
-    int result;
-    
-    mldl_cfg->requested_sensors = sensors;
-    result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = ioctl((int)mlsl_handle, MPU_RESUME, NULL);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors);
-
-    return result;
-}
-
-
-int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, 
-          	    void *mlsl_handle,
-                    void *accel_handle,
-                    void *compass_handle,
-                    void *pressure_handle,
-                    unsigned long sensors)
-{
-    int result;
-    unsigned long requested = mldl_cfg->requested_sensors;
-
-    mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
-    //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__,
-    //         mldl_cfg->requested_sensors);
-
-    result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = ioctl((int)mlsl_handle, MPU_SUSPEND, NULL);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    mldl_cfg->requested_sensors = requested;
-    //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested);
-
-    return result;
-}
-
-/**
- * Send slave configuration information
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param slave slave description
- * @param pdata slave platform data
- * @param data where to store the read data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
-		void *gyro_handle,
-		void *slave_handle,
-		struct ext_slave_descr *slave,
-		struct ext_slave_platform_data *pdata,
-		unsigned char *data)
-{
-    int result;
-    if (!mldl_cfg || !gyro_handle || !data || !slave) {
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    switch (slave->type) {
-    case EXT_SLAVE_TYPE_ACCELEROMETER:
-        result = ioctl((int)gyro_handle, MPU_READ_ACCEL, data);
-        break;
-    case EXT_SLAVE_TYPE_COMPASS:
-        result = ioctl((int)gyro_handle, MPU_READ_COMPASS, data);
-        break;
-    case EXT_SLAVE_TYPE_PRESSURE:
-        result = ioctl((int)gyro_handle, MPU_READ_PRESSURE, data);
-        break;
-    default:
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return result;
-}
-
-/**
- * Send slave configuration information
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param data the data being sent
- * @param slave slave description
- * @param pdata slave platform data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
-                     void *gyro_handle,
-                     void *slave_handle,
-                     struct ext_slave_config *data,
-                     struct ext_slave_descr *slave,
-                     struct ext_slave_platform_data *pdata)
-{
-    int result;
-    if (!mldl_cfg || !data || !slave || !pdata || !slave) {
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    switch (slave->type) {
-    case EXT_SLAVE_TYPE_ACCELEROMETER:
-        result = ioctl((int)gyro_handle, MPU_CONFIG_ACCEL, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case EXT_SLAVE_TYPE_COMPASS:
-        result = ioctl((int)gyro_handle, MPU_CONFIG_COMPASS, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case EXT_SLAVE_TYPE_PRESSURE:
-        result = ioctl((int)gyro_handle, MPU_CONFIG_PRESSURE, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    default:
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-
-    return result;
-}
-
-/**
- * Request slave configuration information
- *
- * Use this specifically after requesting a slave configuration to see what the
- * slave accually accepted.
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param data the data being requested.
- * @param slave slave description
- * @param pdata slave platform data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
-                         void *gyro_handle,
-                         void *slave_handle,
-                         struct ext_slave_config *data,
-                         struct ext_slave_descr *slave,
-                         struct ext_slave_platform_data *pdata)
-{
-    int result;
-    if (!mldl_cfg || !data || !slave) {
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-    switch (slave->type) {
-    case EXT_SLAVE_TYPE_ACCELEROMETER:
-        result = ioctl((int)gyro_handle, MPU_GET_CONFIG_ACCEL, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case EXT_SLAVE_TYPE_COMPASS:
-        result = ioctl((int)gyro_handle, MPU_GET_CONFIG_COMPASS, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    case EXT_SLAVE_TYPE_PRESSURE:
-        result = ioctl((int)gyro_handle, MPU_GET_CONFIG_PRESSURE, data);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        break;
-    default:
-        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
-        return INV_ERROR_INVALID_PARAMETER;
-        break;
-    }
-    return result;
-}
-/**
- *@}
- */
diff --git a/mlsdk/mllite/mldmp.c b/mlsdk/mllite/mldmp.c
deleted file mode 100644
index 200d0d1..0000000
--- a/mlsdk/mllite/mldmp.c
+++ /dev/null
@@ -1,282 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @addtogroup MLDMP
- *
- * @{
- *      @file     mldmp.c
- *      @brief    Shared functions between all the different DMP versions
-**/
-
-#include <stdio.h>
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mltypes.h"
-#include "ml.h"
-#include "mldl_cfg.h"
-#include "mldl.h"
-#include "compass.h"
-#include "mlSetGyroBias.h"
-#include "mlsl.h"
-#include "mlFIFO.h"
-#include "mldmp.h"
-#include "mlstates.h"
-#include "dmpDefault.h"
-#include "mlFIFOHW.h"
-#include "mlsupervisor.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-dmp"
-
-/**
- *  @brief  Open the default motion sensor engine.
- *          This function is used to open the default MPL engine, 
- *          featuring, for example, sensor fusion (6 axes and 9 axes), 
- *          sensor calibration, accelerometer data byte swapping, among 
- *          others.  
- *          Compare with the other provided engines.
- *
- *  @pre    inv_serial_start() must have been called to instantiate the serial 
- *          communication.
- *  
- *  Example:
- *  @code
- *    result = inv_dmp_open( );
- *    if (INV_SUCCESS != result) {
- *        // Handle the error case
- *    }
- *  @endcode
- *
- *  @return Zero on success; Error Code on any failure.
- *
- */
-inv_error_t inv_dmp_open(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    unsigned char state = inv_get_state();
-    struct mldl_cfg *mldl_cfg;
-    unsigned long requested_sensors;
-
-    /*************************************************************
-     * Common operations before calling DMPOpen
-     ************************************************************/
-    if (state == INV_STATE_DMP_OPENED)
-        return INV_SUCCESS;
-
-    if (state == INV_STATE_DMP_STARTED) {
-        return inv_dmp_stop();
-    }
-
-    result = inv_state_transition(INV_STATE_DMP_OPENED);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    result = inv_dl_open(inv_get_serial_handle());
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-#ifdef ML_USE_DMP_SIM
-    do {
-        void setup_univ();
-        setup_univ();           /* hijack the read and write paths 
-                                   and re-direct them to the simulator */
-    } while (0);
-#endif
-
-    result = inv_setup_dmp();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    // Init vars.
-    inv_init_ml();
-
-    result = inv_init_fifo_param();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_enable_set_bias();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    inv_init_fifo_hardare();
-    mldl_cfg = inv_get_dl_config();
-    requested_sensors = INV_THREE_AXIS_GYRO;
-    if (mldl_cfg->accel && mldl_cfg->accel->resume)
-        requested_sensors |= INV_THREE_AXIS_ACCEL;
-
-    if (mldl_cfg->compass && mldl_cfg->compass->resume)
-        requested_sensors |= INV_THREE_AXIS_COMPASS;
-
-    if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
-        requested_sensors |= INV_THREE_AXIS_PRESSURE;
-
-    result = inv_init_requested_sensors(requested_sensors);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_apply_calibration();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_apply_endian_accel();
-
-    return result;
-}
-
-/**
- *  @brief  Start the DMP.
- *
- *  @pre    inv_dmp_open() must have been called.
- * 
- *  @code
- *     result = inv_dmp_start();
- *     if (INV_SUCCESS != result) {
- *         // Handle the error case
- *     }
- *  @endcode
- *
- *  @return INV_SUCCESS if successful, or Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_start(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    if (inv_get_state() == INV_STATE_DMP_STARTED)
-        return INV_SUCCESS;
-
-    result = inv_state_transition(INV_STATE_DMP_STARTED);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    inv_init_sensor_fusion_supervisor();
-    result = inv_dl_start(inv_get_dl_config()->requested_sensors);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    /* This is done after the start since it will modify DMP memory, which 
-     * will cause a full reset is most cases */
-    result = inv_reset_motion();
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  Stops the DMP and puts it in low power.
- *
- *  @pre    inv_dmp_start() must have been called.
- * 
- *  @return INV_SUCCESS, Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_stop(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-
-    if (inv_get_state() == INV_STATE_DMP_OPENED)
-        return INV_SUCCESS;
-
-    result = inv_state_transition(INV_STATE_DMP_OPENED);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_dl_stop(INV_ALL_SENSORS);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  Closes the motion sensor engine.
- *          Does not close the serial communication. To do that,
- *          call inv_serial_stop().
- *          After calling inv_dmp_close() another DMP module can be
- *          loaded in the MPL with the corresponding necessary 
- *          intialization and configurations, via any of the 
- *          MLDmpXXXOpen functions.
- *
- *  @pre    inv_dmp_open() must have been called.
- * 
- *  @code
- *     result = inv_dmp_close();
- *     if (INV_SUCCESS != result) {
- *         // Handle the error case
- *     }
- *  @endcode
- *
- *  @return INV_SUCCESS, Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_close(void)
-{
-    INVENSENSE_FUNC_START;
-    inv_error_t result;
-    inv_error_t firstError = INV_SUCCESS;
-
-    if (inv_get_state() <= INV_STATE_DMP_CLOSED)
-        return INV_SUCCESS;
-
-    result = inv_disable_set_bias();
-    ERROR_CHECK_FIRST(firstError, result);
-
-    result = inv_dl_stop(INV_ALL_SENSORS);
-    ERROR_CHECK_FIRST(firstError, result);
-
-    result = inv_close_fifo();
-    ERROR_CHECK_FIRST(firstError, result);
-
-    result = inv_dl_close();
-    ERROR_CHECK_FIRST(firstError, result);
-
-    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
-    ERROR_CHECK_FIRST(firstError, result);
-
-    return result;
-}
-
-/**
- *  @}
- */
diff --git a/mlsdk/mllite/mldmp.h b/mlsdk/mllite/mldmp.h
deleted file mode 100644
index f519798..0000000
--- a/mlsdk/mllite/mldmp.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/***************************************************************************** *
- * $Id: mldmp.h 5629 2011-06-11 03:13:08Z mcaramello $
- ******************************************************************************/
-
-/**
- * @defgroup MLDMP
- * @brief 
- *
- *  These are the top level functions that define how to load the MPL.  In order
- *  to use most of the features, the DMP must be loaded with some code.  The 
- *  loading procedure takes place when calling inv_dmp_open with a given DMP set 
- *  function, after having open the serial communication with the device via 
- *  inv_serial_start().  
- *  The DMP set function will load the DMP memory and enable a certain
- *  set of features.
- *
- *  First select a DMP version from one of the released DMP sets.  
- *  These could be:
- *  - DMP default to load and use the default DMP code featuring pedometer, 
- *    gestures, and orientation.  Use inv_dmp_open().
- *  - DMP pedometer stand-alone to load and use the standalone pedometer
- *    implementation. Use inv_open_low_power_pedometer().
- *  <!-- - DMP EIS ... Use inv_eis_open_dmp(). -->
- *
- *  After inv_dmp_openXXX any number of appropriate initialization and configuration 
- *  routines can be called. Each one of these routines will return an error code 
- *  and will check to make sure that it is compatible with the the DMP version 
- *  selected during the call to inv_dmp_open.
- *
- *  Once the configuration is complete, make a call to inv_dmp_start(). This will
- *  finally turn on the DMP and run the code previously loaded.
- *
- *  While the DMP is running, all data fetching, polling or other functions can 
- *  be called and will return valid data. Some parameteres can be changed while 
- *  the DMP is runing, while others cannot.  Therefore it is important to always 
- *  check the return code of each function.  Check the error code list in mltypes
- *  to know what each returned error corresponds to.
- *
- *  When no more motion processing is required, the library can be shut down and
- *  the DMP turned off.  We can do that by calling inv_dmp_close().  Note that 
- *  inv_dmp_close() will not close the serial communication automatically, which will
- *  remain open an active, in case another module needs to be loaded instead.
- *  If the intention is shutting down the MPL as well, an explicit call to 
- *  inv_serial_stop() following inv_dmp_close() has to be made.
- *
- *  The MPL additionally implements a basic state machine, whose purpose is to
- *  give feedback to the user on whether he is following all the required 
- *  initialization steps.  If an anomalous transition is detected, the user will
- *  be warned by a terminal message with the format:
- *
- *  <tt>"Error : illegal state transition from STATE_1 to STATE_3"</tt>
- *
- *  @{
- *      @file     mldmp.h
- *      @brief    Top level entry functions to the MPL library with DMP support
- */
-
-#ifndef MLDMP_H
-#define MLDMP_H
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mldmp_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    inv_error_t inv_dmp_open(void);
-    inv_error_t inv_dmp_start(void);
-    inv_error_t inv_dmp_stop(void);
-    inv_error_t inv_dmp_close(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          /* MLDMP_H */
-/**
- * @}
-**/
diff --git a/mlsdk/mllite/mlstates.c b/mlsdk/mllite/mlstates.c
deleted file mode 100644
index 8ebb086..0000000
--- a/mlsdk/mllite/mlstates.c
+++ /dev/null
@@ -1,269 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-/** 
- *  @defgroup MLSTATES
- *  @brief  Basic state machine definition and support for the Motion Library.
- *
- *  @{
- *      @file mlstates.c
- *      @brief The Motion Library state machine definition.
- */
-
-#define ML_C
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <stdio.h>
-#include <string.h>
-
-#include "mlstates.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "ml.h"
-#include "mlos.h"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mlstates"
-
-#define _stateDebug(x)          //{x}
-
-#define MAX_STATE_CHANGE_PROCESSES (8)
-
-struct state_callback_obj {
-    int_fast8_t numStateChangeCallbacks;
-    HANDLE mutex;
-    state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES];
-};
-
-static struct state_callback_obj sStateChangeCallbacks = { 0 };
-
-/* --------------- */
-/* -  Functions. - */
-/* --------------- */
-
-static inv_error_t inv_init_state_callbacks(void)
-{
-    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
-    return inv_create_mutex(&sStateChangeCallbacks.mutex);
-}
-
-static inv_error_t MLStateCloseCallbacks(void)
-{
-    inv_error_t result;
-    result = inv_destroy_mutex(sStateChangeCallbacks.mutex);
-    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  return a string containing the label assigned to the given state.
- *  @param  state   The state of which the label has to be returned.
- *  @return A string containing the state label.
-**/
-char *inv_state_name(unsigned char state)
-{
-    switch (state) {
-    case INV_STATE_SERIAL_CLOSED:
-        return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED);
-        break;
-    case INV_STATE_SERIAL_OPENED:
-        return INV_STATE_NAME(INV_STATE_SERIAL_OPENED);
-        break;
-    case INV_STATE_DMP_OPENED:
-        return INV_STATE_NAME(INV_STATE_DMP_OPENED);
-        break;
-    case INV_STATE_DMP_STARTED:
-        return INV_STATE_NAME(INV_STATE_DMP_STARTED);
-        break;
-    default:
-        return NULL;
-    }
-}
-
-/**
- *  @internal
- *  @brief  Perform a transition from the current state to newState.
- *          Check for the correctness of the transition.
- *          Print out an error message if the transition is illegal .
- *          This routine is also called if a certain normally constant parameters
- *          are changed such as the FIFO Rate.
- *  @param  newState    state we are transitioning to.
- *  @return  
-**/
-inv_error_t inv_state_transition(unsigned char newState)
-{
-    inv_error_t result = INV_SUCCESS;
-
-    if (newState == INV_STATE_SERIAL_CLOSED) {
-        // Always allow transition to closed
-    } else if (newState == INV_STATE_SERIAL_OPENED) {
-        inv_init_state_callbacks(); // Always allow first transition to start over
-    } else if (((newState == INV_STATE_DMP_OPENED) &&
-                ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) ||
-                 (inv_params_obj.state == INV_STATE_DMP_STARTED)))
-               ||
-               ((newState == INV_STATE_DMP_STARTED) &&
-                (inv_params_obj.state == INV_STATE_DMP_OPENED))) {
-        // Valid transitions but no special action required
-    } else {
-        // All other combinations are illegal
-        MPL_LOGE("Error : illegal state transition from %s to %s\n",
-                 inv_state_name(inv_params_obj.state),
-                 inv_state_name(newState));
-        result = INV_ERROR_SM_TRANSITION;
-    }
-
-    if (result == INV_SUCCESS) {
-        _stateDebug(MPL_LOGV
-                    ("ML State transition from %s to %s\n",
-                     inv_state_name(inv_params_obj.state),
-                     inv_state_name(newState)));
-        result = inv_run_state_callbacks(newState);
-        if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) {
-            MLStateCloseCallbacks();
-        }
-        inv_params_obj.state = newState;
-    }
-    return result;
-}
-
-/**
- *  @internal
- *  @brief  To be moved in mlstates.c
-**/
-unsigned char inv_get_state(void)
-{
-    return (inv_params_obj.state);
-}
-
-/**
- * @internal
- * @brief   This registers a function to be called each time the state 
- *          changes. It may also be called when the FIFO Rate is changed.
- *          It will be called at the start of a state change before the
- *          state change has taken place. See Also inv_unregister_state_callback()
- *          The FIFO does not have to be on for this callback.
- * @param func Function to be called when a DMP interrupt occurs.
- * @return INV_SUCCESS or non-zero error code.
- */
-
-inv_error_t inv_register_state_callback(state_change_callback_t callback)
-{
-    INVENSENSE_FUNC_START;
-    int kk;
-    inv_error_t result;
-
-    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
-    if (INV_SUCCESS != result) {
-        return result;
-    }
-    // Make sure we have not filled up our number of allowable callbacks
-    if (sStateChangeCallbacks.numStateChangeCallbacks <
-        MAX_STATE_CHANGE_PROCESSES) {
-        // Make sure we haven't registered this function already
-        for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
-            if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
-                result = INV_ERROR_INVALID_PARAMETER;
-                break;
-            }
-        }
-
-        if (INV_SUCCESS == result) {
-            // Add new callback
-            sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks.
-                                                       numStateChangeCallbacks]
-                = callback;
-            sStateChangeCallbacks.numStateChangeCallbacks++;
-        }
-    } else {
-        result = INV_ERROR_MEMORY_EXAUSTED;
-    }
-
-    inv_unlock_mutex(sStateChangeCallbacks.mutex);
-    return result;
-}
-
-/**
- * @internal
- * @brief   This unregisters a function to be called each time the state 
- *          changes. See Also inv_register_state_callback()
- *          The FIFO does not have to be on for this callback.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_unregister_state_callback(state_change_callback_t callback)
-{
-    INVENSENSE_FUNC_START;
-    int kk, jj;
-    inv_error_t result;
-
-    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
-    if (INV_SUCCESS != result) {
-        return result;
-    }
-    // Make sure we haven't registered this function already
-    result = INV_ERROR_INVALID_PARAMETER;
-    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
-        if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
-            for (jj = kk + 1;
-                 jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) {
-                sStateChangeCallbacks.stateChangeCallbacks[jj - 1] =
-                    sStateChangeCallbacks.stateChangeCallbacks[jj];
-            }
-            sStateChangeCallbacks.numStateChangeCallbacks--;
-            result = INV_SUCCESS;
-            break;
-        }
-    }
-
-    inv_unlock_mutex(sStateChangeCallbacks.mutex);
-    return result;
-}
-
-inv_error_t inv_run_state_callbacks(unsigned char newState)
-{
-    int kk;
-    inv_error_t result;
-
-    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
-    if (INV_SUCCESS != result) {
-        MPL_LOGE("MLOsLockMutex returned %d\n", result);
-        return result;
-    }
-
-    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
-        if (sStateChangeCallbacks.stateChangeCallbacks[kk]) {
-            result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState);
-            if (INV_SUCCESS != result) {
-                break;          // Can't return, must release mutex
-            }
-        }
-    }
-
-    inv_unlock_mutex(sStateChangeCallbacks.mutex);
-    return result;
-}
diff --git a/mlsdk/mllite/mlstates.h b/mlsdk/mllite/mlstates.h
deleted file mode 100644
index cbaa610..0000000
--- a/mlsdk/mllite/mlstates.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: mlstates.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef INV_STATES_H__
-#define INV_STATES_H__
-
-#include "mltypes.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlstates_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* See inv_state_transition for the transition mask */
-#define INV_STATE_SERIAL_CLOSED      (0)
-#define INV_STATE_SERIAL_OPENED      (1)
-#define INV_STATE_DMP_OPENED         (2)
-#define INV_STATE_DMP_STARTED        (3)
-#define INV_STATE_DMP_STOPPED        (INV_STATE_DMP_OPENED)
-#define INV_STATE_DMP_CLOSED         (INV_STATE_SERIAL_OPENED)
-
-#define INV_STATE_NAME(x)            (#x)
-
-    typedef inv_error_t(*state_change_callback_t) (unsigned char newState);
-
-    char *inv_state_name(unsigned char x);
-    inv_error_t inv_state_transition(unsigned char newState);
-    unsigned char inv_get_state(void);
-    inv_error_t inv_register_state_callback(state_change_callback_t callback);
-    inv_error_t inv_unregister_state_callback(state_change_callback_t callback);
-    inv_error_t inv_run_state_callbacks(unsigned char newState);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // INV_STATES_H__
diff --git a/mlsdk/mllite/mlsupervisor.c b/mlsdk/mllite/mlsupervisor.c
deleted file mode 100644
index 139297f..0000000
--- a/mlsdk/mllite/mlsupervisor.c
+++ /dev/null
@@ -1,597 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- *  @defgroup   ML_SUPERVISOR
- *  @brief      Basic sensor fusion supervisor functionalities.
- *
- *  @{
- *      @file   mlsupervisor.c
- *      @brief  Basic sensor fusion supervisor functionalities.
- */
-
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "pressure.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "mlMathFunc.h"
-#include "mlsupervisor.h"
-#include "mlmath.h"
-
-#include "mlsl.h"
-#include "mlos.h"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-sup"
-
-static unsigned long lastCompassTime = 0;
-static unsigned long polltime = 0;
-static unsigned long coiltime = 0;
-static int accCount = 0;
-static int compassCalStableCount = 0;
-static int compassCalCount = 0;
-static int coiltimerstart = 0;
-static unsigned long disturbtime = 0;
-static int disturbtimerstart = 0;
-
-static yas_filter_if_s f;
-static yas_filter_handle_t handle;
-
-#define SUPERVISOR_DEBUG 0
-
-struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 };
-
-/**
- *  @brief  This initializes all variables that should be reset on
- */
-void inv_init_sensor_fusion_supervisor(void)
-{
-    lastCompassTime = 0;
-    polltime = 0;
-    inv_obj.acc_state = SF_STARTUP_SETTLE;
-    accCount = 0;
-    compassCalStableCount = 0;
-    compassCalCount = 0;
-
-    yas_filter_init(&f);
-    f.init(&handle);
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
-	defined CONFIG_MPU_SENSORS_MPU6050B1
-    if (inv_compass_present()) {
-        struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-        if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) {
-            (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT);
-        }
-    }
-#endif
-
-    if (ml_supervisor_cb.supervisor_reset_func != NULL) {
-        ml_supervisor_cb.supervisor_reset_func();
-    }
-}
-
-static int MLUpdateCompassCalibration3DOF(int command, long *data,
-                                          unsigned long deltaTime)
-{
-    INVENSENSE_FUNC_START;
-    int retValue = INV_SUCCESS;
-    static float m[10][10] = { {0} };
-    float mInv[10][10] = { {0} };
-    float mTmp[10][10] = { {0} };
-    static float xTransY[4] = { 0 };
-    float magSqr = 0;
-    float inpData[3] = { 0 };
-    int i, j;
-    int a, b;
-    float d;
-
-    switch (command) {
-    case CAL_ADD_DATA:
-        inpData[0] = (float)data[0];
-        inpData[1] = (float)data[1];
-        inpData[2] = (float)data[2];
-        m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]);
-        m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]);
-        m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]);
-        m[0][3] += (-2 * inpData[0]);
-        m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]);
-        m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]);
-        m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]);
-        m[1][3] += (-2 * inpData[1]);
-        m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]);
-        m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]);
-        m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]);
-        m[2][3] += (-2 * inpData[2]);
-        m[3][0] += (-2 * inpData[0]);
-        m[3][1] += (-2 * inpData[1]);
-        m[3][2] += (-2 * inpData[2]);
-        m[3][3] += 1.0f;
-        magSqr =
-            inpData[0] * inpData[0] + inpData[1] * inpData[1] +
-            inpData[2] * inpData[2];
-        xTransY[0] += (-2 * inpData[0]) * magSqr;
-        xTransY[1] += (-2 * inpData[1]) * magSqr;
-        xTransY[2] += (-2 * inpData[2]) * magSqr;
-        xTransY[3] += magSqr;
-        break;
-    case CAL_RUN:
-        a = 4;
-        b = a;
-        for (i = 0; i < b; i++) {
-            for (j = 0; j < b; j++) {
-                a = b;
-                inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j);
-                mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a);
-            }
-        }
-        a = b;
-        d = inv_matrix_det(&m[0][0], &a);
-        if (d == 0) {
-            return INV_ERROR;
-        }
-        for (i = 0; i < 3; i++) {
-            float tmp = 0;
-            for (j = 0; j < 4; j++) {
-                tmp += mInv[j][i] / d * xTransY[j];
-            }
-            inv_obj.compass_test_bias[i] =
-                -(long)(tmp * inv_obj.compass_sens / 16384.0f);
-        }
-        break;
-    case CAL_RESET:
-        for (i = 0; i < 4; i++) {
-            for (j = 0; j < 4; j++) {
-                m[i][j] = 0;
-            }
-            xTransY[i] = 0;
-        }
-    default:
-        break;
-    }
-    return retValue;
-}
-
-/**
- * Entry point for Sensor Fusion operations
- * @internal
- * @param magFB magnetormeter FB
- * @param accSF accelerometer SF
- */
-static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF,
-                                            unsigned long deltaTime)
-{
-    static long prevCompassBias[3] = { 0 };
-    static long magMax[3] = {
-        -1073741824L,
-        -1073741824L,
-        -1073741824L
-    };
-    static long magMin[3] = {
-        1073741824L,
-        1073741824L,
-        1073741824L
-    };
-    int gyroMag;
-    long accMag;
-    inv_error_t result;
-    int i;
-
-    if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) {
-        ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime);
-    }
-
-    gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT;
-    accMag = inv_accel_sum_of_sqr();
-
-    // Scaling below assumes certain scaling.
-#if ACC_MAG_SQR_SHIFT != 16
-#error
-#endif
-
-    if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) {
-        result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) {
-        //Most basic compass calibration, used only with lite MPL
-        if (inv_obj.resetting_compass == 1) {
-            for (i = 0; i < 3; i++) {
-                magMax[i] = -1073741824L;
-                magMin[i] = 1073741824L;
-                prevCompassBias[i] = 0;
-            }
-            compassCalStableCount = 0;
-            compassCalCount = 0;
-            inv_obj.resetting_compass = 0;
-        }
-        if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) {
-            if (compassCalStableCount < 1000) {
-                for (i = 0; i < 3; i++) {
-                    if (inv_obj.compass_sensor_data[i] > magMax[i]) {
-                        magMax[i] = inv_obj.compass_sensor_data[i];
-                    }
-                    if (inv_obj.compass_sensor_data[i] < magMin[i]) {
-                        magMin[i] = inv_obj.compass_sensor_data[i];
-                    }
-                }
-                MLUpdateCompassCalibration3DOF(CAL_ADD_DATA,
-                                               inv_obj.compass_sensor_data,
-                                               deltaTime);
-                compassCalStableCount += deltaTime;
-                for (i = 0; i < 3; i++) {
-                    if (magMax[i] - magMin[i] <
-                        (long long)40 * 1073741824 / inv_obj.compass_sens) {
-                        compassCalStableCount = 0;
-                    }
-                }
-            } else {
-                if (compassCalStableCount >= 1000) {
-                    if (MLUpdateCompassCalibration3DOF
-                        (CAL_RUN, inv_obj.compass_sensor_data,
-                         deltaTime) == INV_SUCCESS) {
-                        inv_obj.got_compass_bias = 1;
-                        inv_obj.compass_accuracy = 3;
-                        for(i=0; i<3; i++) {
-                            inv_obj.compass_bias_error[i] = 35;
-                        }
-                        if (inv_obj.compass_state == SF_UNCALIBRATED)
-                            inv_obj.compass_state = SF_STARTUP_SETTLE;
-                        inv_set_compass_bias(inv_obj.compass_test_bias);
-                    }
-                    compassCalCount = 0;
-                    compassCalStableCount = 0;
-                    for (i = 0; i < 3; i++) {
-                        magMax[i] = -1073741824L;
-                        magMin[i] = 1073741824L;
-                        prevCompassBias[i] = 0;
-                    }
-                    MLUpdateCompassCalibration3DOF(CAL_RESET,
-                                                   inv_obj.compass_sensor_data,
-                                                   deltaTime);
-                }
-            }
-        }
-        compassCalCount += deltaTime;
-        if (compassCalCount > 20000) {
-            compassCalCount = 0;
-            compassCalStableCount = 0;
-            for (i = 0; i < 3; i++) {
-                magMax[i] = -1073741824L;
-                magMin[i] = 1073741824L;
-                prevCompassBias[i] = 0;
-            }
-            MLUpdateCompassCalibration3DOF(CAL_RESET,
-                                           inv_obj.compass_sensor_data,
-                                           deltaTime);
-        }
-    }
-
-    if (inv_obj.acc_state != SF_STARTUP_SETTLE) {
-        if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) {
-            inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing
-            accCount = 0;
-        } else {
-            if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214)
-                && ((inv_obj.acc_state == SF_DISTURBANCE)
-                    || (inv_obj.acc_state == SF_SLOW_SETTLE))) {
-                accCount += deltaTime;
-                if (accCount > 0) {
-                    inv_obj.acc_state = SF_FAST_SETTLE;
-                    accCount = 0;
-                }
-            } else {
-                if (inv_obj.acc_state == SF_DISTURBANCE) {
-                    accCount += deltaTime;
-                    if (accCount > 500) {
-                        inv_obj.acc_state = SF_SLOW_SETTLE;
-                        accCount = 0;
-                    }
-                } else if (inv_obj.acc_state == SF_SLOW_SETTLE) {
-                    accCount += deltaTime;
-                    if (accCount > 1000) {
-                        inv_obj.acc_state = SF_NORMAL;
-                        accCount = 0;
-                    }
-                } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
-                    accCount += deltaTime;
-                    if (accCount > 250) {
-                        inv_obj.acc_state = SF_NORMAL;
-                        accCount = 0;
-                    }
-                }
-            }
-        }
-    }
-    if (inv_obj.acc_state == SF_STARTUP_SETTLE) {
-        accCount += deltaTime;
-        if (accCount > 50) {
-            *accSF = 1073741824;    //Startup settling
-            inv_obj.acc_state = SF_NORMAL;
-            accCount = 0;
-        }
-    } else if ((inv_obj.acc_state == SF_NORMAL)
-               || (inv_obj.acc_state == SF_SLOW_SETTLE)) {
-        *accSF = inv_obj.accel_sens * 64;   //Normal
-    } else if ((inv_obj.acc_state == SF_DISTURBANCE)) {
-        *accSF = inv_obj.accel_sens * 64;   //Don't use accel (should be 0)
-    } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
-        *accSF = inv_obj.accel_sens * 512;  //Amplify accel
-    }
-    if (!inv_get_gyro_present()) {
-        *accSF = inv_obj.accel_sens * 128;
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Entry point for software sensor fusion operations.
- *          Manages hardware interaction, calls sensor fusion supervisor for
- *          bias calculation.
- *  @return error code. INV_SUCCESS if no error occurred.
- */
-inv_error_t inv_accel_compass_supervisor(void)
-{
-    inv_error_t result;
-    int adjustSensorFusion = 0;
-    long accSF = 1073741824;
-    static double magFB = 0;
-    long magSensorData[3];
-    float fcin[3];
-    float fcout[3];
-    
-
-    if (inv_compass_present()) {    /* check for compass data */
-        int i, j;
-        long long tmp[3] = { 0 };
-        long long tmp64 = 0;
-        unsigned long ctime = inv_get_tick_count();
-        if (((inv_get_compass_id() == COMPASS_ID_AK8975) &&
-             ((ctime - polltime) > 20)) ||
-            (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max
-            if (SUPERVISOR_DEBUG) {
-                MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n");
-                MPL_LOGV("delta time = %ld\n", ctime - polltime);
-            }
-            polltime = ctime;
-            result = inv_get_compass_data(magSensorData);
-            /* external slave wants the data even if there is an error */
-            if (result && !inv_obj.external_slave_callback) {
-                if (SUPERVISOR_DEBUG) {
-                    MPL_LOGW("inv_get_compass_data returned %d\n", result);
-                }
-            } else {
-                unsigned long compassTime = inv_get_tick_count();
-                unsigned long deltaTime = 1;
-
-                if (result == INV_SUCCESS) {
-                    if (lastCompassTime != 0) {
-                        deltaTime = compassTime - lastCompassTime;
-                    }
-                    lastCompassTime = compassTime;
-                    adjustSensorFusion = 1;
-                    if (inv_obj.got_init_compass_bias == 0) {
-                        inv_obj.got_init_compass_bias = 1;
-                        for (i = 0; i < 3; i++) {
-                            inv_obj.init_compass_bias[i] = magSensorData[i];
-                        }
-                    }
-                    for (i = 0; i < 3; i++) {
-                        inv_obj.compass_sensor_data[i] = (long)magSensorData[i];
-                        inv_obj.compass_sensor_data[i] -=
-                            inv_obj.init_compass_bias[i];
-                        tmp[i] = ((long long)inv_obj.compass_sensor_data[i])
-                            * inv_obj.compass_sens / 16384;
-                        tmp[i] -= inv_obj.compass_bias[i];
-                        tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L;
-                    }
-                    for (i = 0; i < 3; i++) {
-                        tmp64 = 0;
-                        for (j = 0; j < 3; j++) {
-                            tmp64 += (long long) tmp[j] *
-                                inv_obj.compass_cal[i * 3 + j];
-                        }
-                        inv_obj.compass_calibrated_data[i] =
-                            (long) (tmp64 / inv_obj.compass_sens);
-                    }
-                    //Additions:
-                    if ((inv_obj.compass_state == 1) &&
-                            (inv_obj.compass_overunder == 0)) {
-                        if (disturbtimerstart == 0) {
-                            disturbtimerstart = 1;
-                            disturbtime = ctime;
-                        }
-                    } else {
-                        disturbtimerstart = 0;
-                    }
-
-                    if (inv_obj.compass_overunder) {
-                        if (coiltimerstart == 0) {
-                            coiltimerstart = 1;
-                            coiltime = ctime;
-                        }
-                    }
-                    if (coiltimerstart == 1) {
-                        if (ctime - coiltime > 3000) {
-                            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
-                            inv_set_compass_offset();
-                            inv_reset_compass_calibration();
-                            coiltimerstart = 0;
-                        }
-                    }
-
-                    if (disturbtimerstart == 1) {
-                        if (ctime - disturbtime > 10000) {
-                            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
-                            inv_set_compass_offset();
-                            inv_reset_compass_calibration();
-                            MPL_LOGI("Compass reset! inv_reset_compass_calibration \n");
-                            disturbtimerstart = 0;
-                        }
-                    }
-                }
-                if (inv_obj.external_slave_callback) {
-                    result = inv_obj.external_slave_callback(&inv_obj);
-                    if (result) {
-                        LOG_RESULT_LOCATION(result);
-                        return result;
-                    }
-                }
-
-#ifdef APPLY_COMPASS_FILTER
-                if (inv_get_compass_id() == COMPASS_ID_YAS530)
-                {
-                    fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f);
-                    fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f);
-                    fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f);
-
-                    f.update(&handle, fcin, fcout);
-
-                    inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f);
-                    inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f);
-                    inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f);
-                }
-#endif
-
-                if (SUPERVISOR_DEBUG) {
-                    MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n",
-                             (float)inv_obj.compass_calibrated_data[0] /
-                             65536.f,
-                             (float)inv_obj.compass_calibrated_data[1] /
-                             65536.f,
-                             (float)inv_obj.compass_calibrated_data[2] /
-                             65536.f);
-                }
-                magFB = 1.0;
-                adjustSensorFusion = 1;
-                result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
-                if (result) {
-                    LOG_RESULT_LOCATION(result);
-                    return result;
-                }
-            }
-        }
-    } else {
-        //No compass, but still modify accel
-        unsigned long ctime = inv_get_tick_count();
-        if (polltime == 0 || ((ctime - polltime) > 80)) {   // at the beginning AND every 1/8 second
-            unsigned long deltaTime = 1;
-            adjustSensorFusion = 1;
-            magFB = 0;
-            if (polltime != 0) {
-                deltaTime = ctime - polltime;
-            }
-            MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
-            polltime = ctime;
-        }
-    }
-    if (adjustSensorFusion == 1) {
-        unsigned char regs[4];
-        static int prevAccSF = 1;
-
-        if (accSF != prevAccSF) {
-            regs[0] = (unsigned char)((accSF >> 24) & 0xff);
-            regs[1] = (unsigned char)((accSF >> 16) & 0xff);
-            regs[2] = (unsigned char)((accSF >> 8) & 0xff);
-            regs[3] = (unsigned char)(accSF & 0xff);
-            result = inv_set_mpu_memory(KEY_D_0_96, 4, regs);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            prevAccSF = accSF;
-        }
-    }
-
-    if (ml_supervisor_cb.accel_compass_fusion_func != NULL)
-        ml_supervisor_cb.accel_compass_fusion_func(magFB);
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Entry point for software sensor fusion operations.
- *          Manages hardware interaction, calls sensor fusion supervisor for
- *          bias calculation.
- *  @return INV_SUCCESS or non-zero error code on error.
- */
-inv_error_t inv_pressure_supervisor(void)
-{
-    long pressureSensorData[1];
-    static unsigned long pressurePolltime = 0;
-    if (inv_pressure_present()) {   /* check for pressure data */
-        unsigned long ctime = inv_get_tick_count();
-        if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second
-            if (SUPERVISOR_DEBUG) {
-                MPL_LOGV("Fetch pressure data\n");
-                MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime);
-            }
-            pressurePolltime = ctime;
-            if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) {
-                inv_obj.pressure = pressureSensorData[0];
-            }
-        }
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  Resets the magnetometer calibration algorithm.
- *  @return INV_SUCCESS if successful, or non-zero error code otherwise.
- */
-inv_error_t inv_reset_compass_calibration(void)
-{
-    if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) {
-        if (ml_supervisor_cb.reset_advanced_compass_func != NULL)
-            ml_supervisor_cb.reset_advanced_compass_func();
-    }
-    MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1);
-
-    inv_obj.compass_bias_error[0] = P_INIT;
-    inv_obj.compass_bias_error[1] = P_INIT;
-    inv_obj.compass_bias_error[2] = P_INIT;
-    inv_obj.compass_accuracy = 0;
-
-    inv_obj.got_compass_bias = 0;
-    inv_obj.got_init_compass_bias = 0;
-    inv_obj.compass_state = SF_UNCALIBRATED;
-    inv_obj.resetting_compass = 1;
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @}
- */
diff --git a/mlsdk/mllite/mlsupervisor.h b/mlsdk/mllite/mlsupervisor.h
deleted file mode 100644
index 62b427e..0000000
--- a/mlsdk/mllite/mlsupervisor.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mlsupervisor.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *****************************************************************************/
-
-#ifndef __INV_SUPERVISOR_H__
-#define __INV_SUPERVISOR_H__
-
-#include "mltypes.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlsupervisor_legacy.h"
-#endif
-
-// The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = 2^this_number
-// this number must be >=0 and even.
-#define GYRO_MAG_SQR_SHIFT 6
-// The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = 2^this_number
-#define ACC_MAG_SQR_SHIFT 16
-
-#define CAL_RUN             0
-#define CAL_RESET           1
-#define CAL_CHANGED_DATA    2
-#define CAL_RESET_TIME      3
-#define CAL_ADD_DATA        4
-#define CAL_COMBINE         5
-
-#define P_INIT  100000
-
-#define SF_NORMAL           0
-#define SF_DISTURBANCE      1
-#define SF_FAST_SETTLE      2
-#define SF_SLOW_SETTLE      3
-#define SF_STARTUP_SETTLE   4
-#define SF_UNCALIBRATED     5
-
-struct inv_supervisor_cb_obj {
-    void (*accel_compass_fusion_func) (double magFB);
-     inv_error_t(*progressive_no_motion_supervisor_func) (unsigned long
-                                                          deltaTime);
-     inv_error_t(*sensor_fusion_advanced_func) (double *magFB,
-                                                unsigned long deltaTime);
-    void (*reset_advanced_compass_func) (void);
-    void (*supervisor_reset_func) (void);
-};
-
-inv_error_t inv_reset_compass_calibration(void);
-void inv_init_sensor_fusion_supervisor(void);
-inv_error_t inv_accel_compass_supervisor(void);
-inv_error_t inv_pressure_supervisor(void);
-
-#endif // __INV_SUPERVISOR_H__
-
diff --git a/mlsdk/mllite/pressure.c b/mlsdk/mllite/pressure.c
deleted file mode 100644
index 9c162ec..0000000
--- a/mlsdk/mllite/pressure.c
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: pressure.c 4120 2010-11-21 19:56:16Z mcaramello $
- *
- *******************************************************************************/
-
-/** 
- *  @defgroup PRESSUREDL 
- *  @brief  Motion Library - Pressure Driver Layer.
- *          Provides the interface to setup and handle a pressure sensor
- *          connected to either the primary or the seconday I2C interface 
- *          of the gyroscope.
- *
- *  @{
- *      @file   pressure.c
- *      @brief  Pressure setup and handling methods.
-**/
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "pressure.h"
-
-#include "ml.h"
-#include "mlinclude.h"
-#include "dmpKey.h"
-#include "mlFIFO.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mlMathFunc.h"
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-pressure"
-
-#define _pressureDebug(x)       //{x}
-
-/* --------------------- */
-/* - Global Variables. - */
-/* --------------------- */
-
-/* --------------------- */
-/* - Static Variables. - */
-/* --------------------- */
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* -------------- */
-/* - Externs.   - */
-/* -------------- */
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/** 
- *  @brief  Is a pressure configured and used by MPL?
- *  @return INV_SUCCESS if the pressure is present.
- */
-unsigned char inv_pressure_present(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->pressure &&
-        NULL != mldl_cfg->pressure->resume &&
-        mldl_cfg->requested_sensors & INV_THREE_AXIS_PRESSURE)
-        return TRUE;
-    else
-        return FALSE;
-}
-
-/**
- *  @brief   Query the pressure slave address.
- *  @return  The 7-bit pressure slave address.
- */
-unsigned char inv_get_pressure_slave_addr(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->pdata)
-        return mldl_cfg->pdata->pressure.address;
-    else
-        return 0;
-}
-
-/**
- *  @brief   Get the ID of the pressure in use.
- *  @return  ID of the pressure in use.
- */
-unsigned short inv_get_pressure_id(void)
-{
-    INVENSENSE_FUNC_START;
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-    if (NULL != mldl_cfg->pressure) {
-        return mldl_cfg->pressure->id;
-    }
-    return ID_INVALID;
-}
-
-/**
- *  @brief  Get a sample of pressure data from the device.
- *  @param  data
- *              the buffer to store the pressure raw data for
- *              X, Y, and Z axes.
- *  @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_pressure_data(long *data)
-{
-    inv_error_t result = INV_SUCCESS;
-    unsigned char tmp[3];
-    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
-    /*--- read the pressure sensor data.
-          The pressure read function may return an INV_ERROR_PRESSURE_* errors
-          when the data is not ready (read/refresh frequency mismatch) or 
-          the internal data sampling timing of the device was not respected. 
-          Returning the error code will make the sensor fusion supervisor 
-          ignore this pressure data sample. ---*/
-    result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg,
-                                                 inv_get_serial_handle(),
-                                                 inv_get_serial_handle(), tmp);
-    if (result) {
-        _pressureDebug(MPL_LOGV
-                       ("inv_mpu_read_pressure returned %d (%s)\n", result,
-                        MLErrorCode(result)));
-        return result;
-    }
-    if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->pressure->endian)
-        data[0] =
-            (((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) +
-            ((long)tmp[2]);
-    else
-        data[0] =
-            (((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) +
-            ((long)tmp[0]);
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @}
- */
diff --git a/mlsdk/mllite/pressure.h b/mlsdk/mllite/pressure.h
deleted file mode 100644
index 77c5d43..0000000
--- a/mlsdk/mllite/pressure.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: pressure.h 4092 2010-11-17 23:49:22Z kkeal $
- *
- *******************************************************************************/
-
-#ifndef PRESSURE_H
-#define PRESSURE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "pressure_legacy.h"
-#endif
-
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-#define USE_PRESSURE_BMA                    0
-
-#define PRESSURE_SLAVEADDR_INVALID          0x00
-#define PRESSURE_SLAVEADDR_BMA085           0x77
-
-/*
-    Define default pressure to use if no selection is made
-*/
-#if USE_PRESSURE_BMA
-#define DEFAULT_PRESSURE_TYPE              PRESSURE_ID_BMA
-#endif
-
-    /* --------------- */
-    /* - Structures. - */
-    /* --------------- */
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-
-    unsigned char inv_pressure_present(void);
-    unsigned char inv_get_pressure_slave_addr(void);
-    inv_error_t inv_suspend_pressure(void);
-    inv_error_t inv_resume_presure(void);
-    inv_error_t inv_get_pressure_data(long *data);
-    unsigned short inv_get_pressure_id(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          // PRESSURE_H
diff --git a/mlsdk/mlutils/checksum.c b/mlsdk/mlutils/checksum.c
deleted file mode 100644
index a97477d..0000000
--- a/mlsdk/mlutils/checksum.c
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "mltypes.h"
-
-/** bernstein hash, from public domain source */
-
-uint32_t inv_checksum(unsigned char *str, int len)
-{
-    uint32_t hash = 5381;
-    int i, c;
-
-    for (i = 0; i < len; i++) {
-        c = *(str + i);
-        hash = ((hash << 5) + hash) + c;    /* hash * 33 + c */
-    }
-
-    return hash;
-}
diff --git a/mlsdk/mlutils/checksum.h b/mlsdk/mlutils/checksum.h
deleted file mode 100644
index 4d3f046..0000000
--- a/mlsdk/mlutils/checksum.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef MPLCHECKSUM_H
-#define MPLCHECKSUM_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "checksum_legacy.h"
-#endif
-
-    uint32_t inv_checksum(unsigned char *str, int len);
-
-#ifdef __cplusplus
-}
-#endif
-#endif                          /* MPLCHECKSUM_H */
diff --git a/mlsdk/mlutils/mputest.c b/mlsdk/mlutils/mputest.c
deleted file mode 100644
index ac0fa30..0000000
--- a/mlsdk/mlutils/mputest.c
+++ /dev/null
@@ -1,1396 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $
- *
- *****************************************************************************/
-
-/*
- *  MPU Self Test functions
- *  Version 2.4
- *  May 13th, 2011
- */
-
-/**
- *  @defgroup MPU_SELF_TEST
- *  @brief  MPU Self Test functions
- *
- *  These functions provide an in-site test of the MPU 3xxx chips. The main
- *      entry point is the inv_mpu_test function.
- *  This runs the tests (as described in the accompanying documentation) and
- *      writes a configuration file containing initial calibration data.
- *  inv_mpu_test returns INV_SUCCESS if the chip passes the tests.
- *  Otherwise, an error code is returned.
- *  The functions in this file rely on MLSL and MLOS: refer to the MPL
- *      documentation for more information regarding the system interface
- *      files.
- *
- *  @{
- *      @file   mputest.c
- *      @brief  MPU Self Test routines for assessing gyro sensor status
- *              after surface mount has happened on the target host platform.
- */
-
-#include <stdio.h>
-#include <time.h>
-#include <string.h>
-#include <math.h>
-#include <stdlib.h>
-#ifdef LINUX
-#include <unistd.h>
-#endif
-
-#include "mpu.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "accel.h"
-#include "mlFIFO.h"
-#include "slave.h"
-#include "ml.h"
-#include "ml_stored_data.h"
-#include "checksum.h"
-
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mpust"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
-    Defines
-*/
-
-#define VERBOSE_OUT 0
-
-/*#define TRACK_IDS*/
-
-/*--- Test parameters defaults. See set_test_parameters for more details ---*/
-
-#define DEF_MPU_ADDR             (0x68)        /* I2C address of the mpu     */
-
-#if (USE_SENSE_PATH_TEST == 1)                 /* gyro full scale dps        */
-#define DEF_GYRO_FULLSCALE       (2000)
-#else
-#define DEF_GYRO_FULLSCALE       (250)
-#endif
-
-#define DEF_GYRO_SENS            (32768.f / DEF_GYRO_FULLSCALE)
-                                               /* gyro sensitivity LSB/dps   */
-#define DEF_PACKET_THRESH        (75)          /* 600 ms / 8ms / sample      */
-#define DEF_TOTAL_TIMING_TOL     (.03f)        /* 3% = 2 pkts + 1% proc tol. */
-#define DEF_BIAS_THRESH          (40 * DEF_GYRO_SENS)
-                                               /* 40 dps in LSBs             */
-#define DEF_RMS_THRESH           (0.4f * DEF_GYRO_SENS)
-                                               /* 0.4 dps-rms in LSB-rms     */
-#define DEF_SP_SHIFT_THRESH_CUST (.05f)        /* 5%                         */
-#define DEF_TEST_TIME_PER_AXIS   (600)         /* ms of time spent collecting
-                                                  data for each axis,
-                                                  multiple of 600ms          */
-#define DEF_N_ACCEL_SAMPLES      (20)          /* num of accel samples to
-                                                  average from, if applic.   */
-
-#define ML_INIT_CAL_LEN          (36)          /* length in bytes of
-                                                  calibration data file      */
-
-/*
-    Macros
-*/
-
-#define CHECK_TEST_ERROR(x)                                                 \
-    if (x) {                                                                \
-        MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__);              \
-        return (-1);                                                        \
-    }
-
-#define SHORT_TO_TEMP_C(shrt)         (((shrt+13200.f)/280.f)+35.f)
-
-#define USHORT_TO_CHARS(chr,shrt)     (chr)[0]=(unsigned char)(shrt>>8);    \
-                                      (chr)[1]=(unsigned char)(shrt);
-
-#define UINT_TO_CHARS(chr,ui)         (chr)[0]=(unsigned char)(ui>>24);     \
-                                      (chr)[1]=(unsigned char)(ui>>16);     \
-                                      (chr)[2]=(unsigned char)(ui>>8);      \
-                                      (chr)[3]=(unsigned char)(ui);
-
-#define FLOAT_TO_SHORT(f)             (                                     \
-                                        (fabs(f-(short)f)>=0.5) ? (         \
-                                            ((short)f)+(f<0?(-1):(+1))) :   \
-                                            ((short)f)                      \
-                                      )
-
-#define CHARS_TO_SHORT(d)             ((((short)(d)[0])<<8)+(d)[1])
-#define CHARS_TO_SHORT_SWAPPED(d)     ((((short)(d)[1])<<8)+(d)[0])
-
-#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5]
-
-#define CHECK_NACKS(d)  (                               \
-                         d[0]==0xff && d[1]==0xff &&    \
-                         d[2]==0xff && d[3]==0xff &&    \
-                         d[4]==0xff && d[5]==0xff       \
-                        )
-
-/*
-    Prototypes
-*/
-
-static inv_error_t test_get_data(
-                    void *mlsl_handle,
-                    struct mldl_cfg *mputestCfgPtr,
-                    short *vals);
-
-/*
-    Types
-*/
-typedef struct {
-    float gyro_sens;
-    int gyro_fs;
-    int packet_thresh;
-    float total_timing_tol;
-    int bias_thresh;
-    float rms_threshSq;
-    float sp_shift_thresh;
-    unsigned int test_time_per_axis;
-    unsigned short accel_samples;
-} tTestSetup;
-
-/*
-    Global variables
-*/
-static unsigned char dataout[20];
-static unsigned char dataStore[ML_INIT_CAL_LEN];
-
-static tTestSetup test_setup = {
-    DEF_GYRO_SENS,
-    DEF_GYRO_FULLSCALE,
-    DEF_PACKET_THRESH,
-    DEF_TOTAL_TIMING_TOL,
-    (int)DEF_BIAS_THRESH,
-    DEF_RMS_THRESH * DEF_RMS_THRESH,
-    DEF_SP_SHIFT_THRESH_CUST,
-    DEF_TEST_TIME_PER_AXIS,
-    DEF_N_ACCEL_SAMPLES
-};
-
-static float adjGyroSens;
-static char a_name[3][2] = {"X", "Y", "Z"};
-
-/*
-    NOTE :  modify get_slave_descr parameter below to reflect
-                the DEFAULT accelerometer in use. The accelerometer in use
-                can be modified at run-time using the inv_test_setup_accel API.
-    NOTE :  modify the expected z axis orientation (Z axis pointing
-                upward or downward)
-*/
-
-signed char g_z_sign = +1;
-struct mldl_cfg *mputestCfgPtr = NULL;
-
-#ifndef LINUX
-/**
- *  @internal
- *  @brief  usec precision sleep function.
- *  @param  number of micro seconds (us) to sleep for.
- */
-static void usleep(unsigned long t)
-{
-    unsigned long start = inv_get_tick_count();
-    while (inv_get_tick_count()-start < t / 1000);
-}
-#endif
-
-/**
- *  @brief  Modify the self test limits from their default values.
- *
- *  @param  slave_addr
- *              the slave address the MPU device is setup to respond at.
- *              The default is DEF_MPU_ADDR = 0x68.
- *  @param  sensitivity
- *              the read sensitivity of the device in LSB/dps as it is trimmed.
- *              NOTE :  if using the self test as part of the MPL, the
- *                      sensitivity the different sensitivity trims are already
- *                      taken care of.
- *  @param  p_thresh
- *              number of packets expected to be received in a 600 ms period.
- *              Depends on the sampling frequency of choice (set by default to
- *              125 Hz) and low pass filter cut-off frequency selection (set
- *              to 42 Hz).
- *              The default is DEF_PACKET_THRESH = 75 packets.
- *  @param  total_time_tol
- *              time skew tolerance, taking into account imprecision in turning
- *              the FIFO on and off and the processor time imprecision (for
- *              1 GHz processor).
- *              The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets.
- *  @param  bias_thresh
- *              bias level threshold, the maximun acceptable no motion bias
- *              for a production quality part.
- *              The default is DEF_BIAS_THRESH = 40 dps.
- *  @param  rms_thresh
- *              the limit standard deviation (=~ RMS) set to assess whether
- *              the noise level on the part is acceptable.
- *              The default is DEF_RMS_THRESH = 0.2 dps-rms.
- *  @param  sp_shift_thresh
- *              the limit shift applicable to the Sense Path self test
- *              calculation.
- */
-void inv_set_test_parameters(unsigned int slave_addr, float sensitivity,
-                             int p_thresh, float total_time_tol,
-                             int bias_thresh, float rms_thresh,
-                             float sp_shift_thresh,
-                             unsigned short accel_samples)
-{
-    mputestCfgPtr->addr = slave_addr;
-    test_setup.gyro_sens = sensitivity;
-    test_setup.gyro_fs = (int)(32768.f / sensitivity);
-    test_setup.packet_thresh = p_thresh;
-    test_setup.total_timing_tol = total_time_tol;
-    test_setup.bias_thresh = bias_thresh;
-    test_setup.rms_threshSq = rms_thresh * rms_thresh;
-    test_setup.sp_shift_thresh = sp_shift_thresh;
-    test_setup.accel_samples = accel_samples;
-}
-
-#define X   (0)
-#define Y   (1)
-#define Z   (2)
-
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-/**
- *  @brief  Test the gyroscope sensor.
- *          Implements the core logic of the MPU Self Test.
- *          Produces the PASS/FAIL result. Loads the calculated gyro biases
- *          and temperature datum into the corresponding pointers.
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  gyro_biases
- *              output pointer to store the initial bias calculation provided
- *              by the MPU Self Test.  Requires 3 elements for gyro X, Y,
- *              and Z.
- *  @param  temp_avg
- *              output pointer to store the initial average temperature as
- *              provided by the MPU Self Test.
- *  @param  perform_full_test
- *              If 1:
- *              calculates offset, drive frequency, and noise and compare it
- *              against set thresholds.
- *              Report also the final result using a bit-mask like error code
- *              as explained in return value description.
- *              When 0:
- *              skip the noise and drive frequency calculation and pass/fail
- *              assessment; simply calculates the gyro biases.
- *
- *  @return 0 on success.
- *          On error, the return value is a bitmask representing:
- *          0, 1, 2     Failures with PLLs on X, Y, Z gyros respectively
- *                      (decimal values will be 1, 2, 4 respectively).
- *          3, 4, 5     Excessive offset with X, Y, Z gyros respectively
- *                      (decimal values will be 8, 16, 32 respectively).
- *          6, 7, 8     Excessive noise with X, Y, Z gyros respectively
- *                      (decimal values will be 64, 128, 256 respectively).
- *          9           If any of the RMS noise values is zero, it could be
- *                      due to a non-functional gyro or FIFO/register failure.
- *                      (decimal value will be 512).
- *                      (decimal values will be 1024, 2048, 4096 respectively).
- */
-int inv_test_gyro_3050(void *mlsl_handle,
-                       short gyro_biases[3], short *temp_avg,
-                       uint_fast8_t perform_full_test)
-{
-    int retVal = 0;
-    inv_error_t result;
-
-    int total_count = 0;
-    int total_count_axis[3] = {0, 0, 0};
-    int packet_count;
-    short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    unsigned char regs[7];
-
-    int temperature;
-    float Avg[3];
-    float RMS[3];
-    int i, j, tmp;
-    char tmpStr[200];
-
-    temperature = 0;
-
-    /* sample rate = 8ms */
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_SMPLRT_DIV, 0x07);
-    CHECK_TEST_ERROR(result);
-
-    regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
-    switch (DEF_GYRO_FULLSCALE) {
-        case 2000:
-            regs[0] |= 0x18;
-            break;
-        case 1000:
-            regs[0] |= 0x10;
-            break;
-        case 500:
-            regs[0] |= 0x08;
-            break;
-        case 250:
-        default:
-            regs[0] |= 0x00;
-            break;
-    }
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_DLPF_FS_SYNC, regs[0]);
-    CHECK_TEST_ERROR(result);
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_INT_CFG, 0x00);
-    CHECK_TEST_ERROR(result);
-
-    /* 1st, timing test */
-    for (j = 0; j < 3; j++) {
-
-        MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
-
-        /* turn on all gyros, use gyro X for clocking
-           Set to Y and Z for 2nd and 3rd iteration */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGM, j + 1);
-        CHECK_TEST_ERROR(result);
-
-        /* wait for 2 ms after switching clock source */
-        usleep(2000);
-
-        /* we will enable XYZ gyro in FIFO and nothing else */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_FIFO_EN2, 0x00);
-        CHECK_TEST_ERROR(result);
-        /* enable/reset FIFO */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
-        CHECK_TEST_ERROR(result);
-
-        tmp = (int)(test_setup.test_time_per_axis / 600);
-        while (tmp-- > 0) {
-            /* enable XYZ gyro in FIFO and nothing else */
-            result = inv_serial_single_write(mlsl_handle,
-                        mputestCfgPtr->addr, MPUREG_FIFO_EN1,
-                        BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
-            CHECK_TEST_ERROR(result);
-
-            /* wait for 600 ms for data */
-            usleep(600000);
-
-            /* stop storing gyro in the FIFO */
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_FIFO_EN1, 0x00);
-            CHECK_TEST_ERROR(result);
-
-            /* Getting number of bytes in FIFO */
-            result = inv_serial_read(
-                           mlsl_handle, mputestCfgPtr->addr,
-                           MPUREG_FIFO_COUNTH, 2, dataout);
-            CHECK_TEST_ERROR(result);
-            /* number of 6 B packets in the FIFO */
-            packet_count = CHARS_TO_SHORT(dataout) / 6;
-            sprintf(tmpStr, "Packet Count: %d - ", packet_count);
-
-            if ( abs(packet_count - test_setup.packet_thresh)
-                        <=      /* Within +/- total_timing_tol % range */
-                     test_setup.total_timing_tol * test_setup.packet_thresh) {
-                for (i = 0; i < packet_count; i++) {
-                    /* getting FIFO data */
-                    result = inv_serial_read_fifo(mlsl_handle,
-                                mputestCfgPtr->addr, 6, dataout);
-                    CHECK_TEST_ERROR(result);
-                    x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
-                    y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
-                    z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
-                    if (VERBOSE_OUT) {
-                        MPL_LOGI("Gyros %-4d    : %+13d %+13d %+13d\n",
-                                    total_count + i, x[total_count + i],
-                                    y[total_count + i], z[total_count + i]);
-                    }
-                }
-                total_count += packet_count;
-                total_count_axis[j] += packet_count;
-                sprintf(tmpStr, "%sOK", tmpStr);
-            } else {
-                if (perform_full_test)
-                    retVal |= 1 << j;
-                sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
-            }
-            MPL_LOGI("%s\n", tmpStr);
-        }
-
-        /* remove gyros from FIFO */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_FIFO_EN1, 0x00);
-        CHECK_TEST_ERROR(result);
-
-        /* Read Temperature */
-        result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_TEMP_OUT_H, 2, dataout);
-        CHECK_TEST_ERROR(result);
-        temperature += (short)CHARS_TO_SHORT(dataout);
-    }
-
-    MPL_LOGI("\n");
-    MPL_LOGI("Total %d samples\n", total_count);
-    MPL_LOGI("\n");
-
-    /* 2nd, check bias from X and Y PLL clock source */
-    tmp = total_count != 0 ? total_count : 1;
-    for (i = 0,
-            Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
-         i < total_count; i++) {
-        Avg[X] += 1.f * x[i] / tmp;
-        Avg[Y] += 1.f * y[i] / tmp;
-        Avg[Z] += 1.f * z[i] / tmp;
-    }
-    MPL_LOGI("bias          : %+13.3f %+13.3f %+13.3f (LSB)\n",
-             Avg[X], Avg[Y], Avg[Z]);
-    if (VERBOSE_OUT) {
-        MPL_LOGI("              : %+13.3f %+13.3f %+13.3f (dps)\n",
-                 Avg[X] / adjGyroSens,
-                 Avg[Y] / adjGyroSens,
-                 Avg[Z] / adjGyroSens);
-    }
-    if(perform_full_test) {
-        for (j = 0; j < 3; j++) {
-            if (fabs(Avg[j]) > test_setup.bias_thresh) {
-                MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
-                         "(threshold = %d)\n",
-                         a_name[j], Avg[j], test_setup.bias_thresh);
-                retVal |= 1 << (3+j);
-            }
-        }
-    }
-
-    /* 3rd, check RMS */
-    if (perform_full_test) {
-        for (i = 0,
-                RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
-             i < total_count; i++) {
-            RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]);
-            RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]);
-            RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]);
-        }
-        for (j = 0; j < 3; j++) {
-            if (RMS[j] > test_setup.rms_threshSq * total_count) {
-                MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold "
-                         "(threshold = %.2f)\n",
-                         a_name[j], sqrt(RMS[j] / total_count),
-                         sqrt(test_setup.rms_threshSq));
-                retVal |= 1 << (6+j);
-            }
-        }
-
-        MPL_LOGI("RMS           : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
-                    sqrt(RMS[X] / total_count),
-                    sqrt(RMS[Y] / total_count),
-                    sqrt(RMS[Z] / total_count));
-        if (VERBOSE_OUT) {
-            MPL_LOGI("RMS ^ 2       : %+13.3f %+13.3f %+13.3f\n",
-                        RMS[X] / total_count,
-                        RMS[Y] / total_count,
-                        RMS[Z] / total_count);
-        }
-
-        if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) {
-            /*  If any of the RMS noise value returns zero,
-                then we might have dead gyro or FIFO/register failure,
-                the part is sleeping, or the part is not responsive */
-            retVal |= 1 << 9;
-        }
-    }
-
-    /* 4th, temperature average */
-    temperature /= 3;
-    if (VERBOSE_OUT)
-        MPL_LOGI("Temperature   : %+13.3f %13s %13s (deg. C)\n",
-                 SHORT_TO_TEMP_C(temperature), "", "");
-
-    /* load into final storage */
-    *temp_avg = (short)temperature;
-    gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
-    gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
-    gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
-
-    return retVal;
-}
-
-#else /* CONFIG_MPU_SENSORS_MPU3050 */
-
-/**
- *  @brief  Test the gyroscope sensor.
- *          Implements the core logic of the MPU Self Test but does not provide
- *          a PASS/FAIL output as in the MPU-3050 implementation.
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  gyro_biases
- *              output pointer to store the initial bias calculation provided
- *              by the MPU Self Test.  Requires 3 elements for gyro X, Y,
- *              and Z.
- *  @param  temp_avg
- *              output pointer to store the initial average temperature as
- *              provided by the MPU Self Test.
- *
- *  @return 0 on success.
- *          A non-zero error code on error.
- */
-int inv_test_gyro_6050(void *mlsl_handle,
-                       short gyro_biases[3], short *temp_avg)
-{
-    inv_error_t result;
-
-    int total_count = 0;
-    int total_count_axis[3] = {0, 0, 0};
-    int packet_count;
-    short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
-    unsigned char regs[7];
-
-    int temperature = 0;
-    float Avg[3];
-    int i, j, tmp;
-    char tmpStr[200];
-
-    /* sample rate = 8ms */
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_SMPLRT_DIV, 0x07);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
-    switch (DEF_GYRO_FULLSCALE) {
-        case 2000:
-            regs[0] |= 0x18;
-            break;
-        case 1000:
-            regs[0] |= 0x10;
-            break;
-        case 500:
-            regs[0] |= 0x08;
-            break;
-        case 250:
-        default:
-            regs[0] |= 0x00;
-            break;
-    }
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_CONFIG, regs[0]);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-    result = inv_serial_single_write(
-                mlsl_handle, mputestCfgPtr->addr,
-                MPUREG_INT_ENABLE, 0x00);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    /* 1st, timing test */
-    for (j = 0; j < 3; j++) {
-        MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
-
-        /* turn on all gyros, use gyro X for clocking
-           Set to Y and Z for 2nd and 3rd iteration */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1));
-#else
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGMT_1, j + 1);
-#endif
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        /* wait for 2 ms after switching clock source */
-        usleep(2000);
-
-        /* enable/reset FIFO */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        tmp = (int)(test_setup.test_time_per_axis / 600);
-        while (tmp-- > 0) {
-            /* enable XYZ gyro in FIFO and nothing else */
-            result = inv_serial_single_write(mlsl_handle,
-                        mputestCfgPtr->addr, MPUREG_FIFO_EN,
-                        BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-
-            /* wait for 600 ms for data */
-            usleep(600000);
-            /* stop storing gyro in the FIFO */
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_FIFO_EN, 0x00);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            /* Getting number of bytes in FIFO */
-            result = inv_serial_read(
-                           mlsl_handle, mputestCfgPtr->addr,
-                           MPUREG_FIFO_COUNTH, 2, dataout);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-            /* number of 6 B packets in the FIFO */
-            packet_count = CHARS_TO_SHORT(dataout) / 6;
-            sprintf(tmpStr, "Packet Count: %d - ", packet_count);
-
-            if (abs(packet_count - test_setup.packet_thresh)
-                        <=      /* Within +/- total_timing_tol % range */
-                     test_setup.total_timing_tol * test_setup.packet_thresh) {
-                for (i = 0; i < packet_count; i++) {
-                    /* getting FIFO data */
-                    result = inv_serial_read_fifo(mlsl_handle,
-                                mputestCfgPtr->addr, 6, dataout);
-                    if (result) {
-                        LOG_RESULT_LOCATION(result);
-                        return result;
-                    }
-                    x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
-                    y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
-                    z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
-                    if (VERBOSE_OUT) {
-                        MPL_LOGI("Gyros %-4d    : %+13d %+13d %+13d\n",
-                                    total_count + i, x[total_count + i],
-                                    y[total_count + i], z[total_count + i]);
-                    }
-                }
-                total_count += packet_count;
-                total_count_axis[j] += packet_count;
-                sprintf(tmpStr, "%sOK", tmpStr);
-            } else {
-                sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
-            }
-            MPL_LOGI("%s\n", tmpStr);
-        }
-
-        /* remove gyros from FIFO */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_FIFO_EN, 0x00);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        /* Read Temperature */
-        result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_TEMP_OUT_H, 2, dataout);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-        temperature += (short)CHARS_TO_SHORT(dataout);
-    }
-
-    MPL_LOGI("\n");
-    MPL_LOGI("Total %d samples\n", total_count);
-    MPL_LOGI("\n");
-
-    /* 2nd, check bias from X and Y PLL clock source */
-    tmp = total_count != 0 ? total_count : 1;
-    for (i = 0,
-            Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
-         i < total_count; i++) {
-        Avg[X] += 1.f * x[i] / tmp;
-        Avg[Y] += 1.f * y[i] / tmp;
-        Avg[Z] += 1.f * z[i] / tmp;
-    }
-    MPL_LOGI("bias          : %+13.3f %+13.3f %+13.3f (LSB)\n",
-             Avg[X], Avg[Y], Avg[Z]);
-    if (VERBOSE_OUT) {
-        MPL_LOGI("              : %+13.3f %+13.3f %+13.3f (dps)\n",
-                 Avg[X] / adjGyroSens,
-                 Avg[Y] / adjGyroSens,
-                 Avg[Z] / adjGyroSens);
-    }
-
-    temperature /= 3;
-    if (VERBOSE_OUT)
-        MPL_LOGI("Temperature   : %+13.3f %13s %13s (deg. C)\n",
-                 SHORT_TO_TEMP_C(temperature), "", "");
-
-    /* load into final storage */
-    *temp_avg = (short)temperature;
-    gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
-    gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
-    gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
-
-    return INV_SUCCESS;
-}
-
-#endif /* CONFIG_MPU_SENSORS_MPU3050 */
-
-#ifdef TRACK_IDS
-/**
- *  @internal
- *  @brief  Retrieve the unique MPU device identifier from the internal OTP
- *          bank 0 memory.
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @return 0 on success, a non-zero error code from the serial layer on error.
- */
-static inv_error_t test_get_mpu_id(void *mlsl_handle)
-{
-    inv_error_t result;
-    unsigned char otp0[8];
-
-
-    result =
-        inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr,
-            (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 |
-            0x00, 6, otp0);
-    if (result)
-        goto close;
-
-    MPL_LOGI("\n");
-    MPL_LOGI("DIE_ID   : %06X\n",
-                ((int)otp0[1] << 8 | otp0[0]) & 0x1fff);
-    MPL_LOGI("WAFER_ID : %06X\n",
-                (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5);
-    MPL_LOGI("A_LOT_ID : %06X\n",
-                ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 |
-                otp0[2]) & 0x3ffff) >> 2);
-    MPL_LOGI("W_LOT_ID : %06X\n",
-                ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2);
-    MPL_LOGI("WP_ID    : %06X\n",
-                ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7);
-    MPL_LOGI("REV_ID   : %06X\n", otp0[6] >> 2);
-    MPL_LOGI("\n");
-
-close:
-    result =
-        inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00);
-    return result;
-}
-#endif /* TRACK_IDS */
-
-/**
- *  @brief  If requested via inv_test_setup_accel(), test the accelerometer biases
- *          and calculate the necessary bias correction.
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  bias
- *              output pointer to store the initial bias calculation provided
- *              by the MPU Self Test.  Requires 3 elements to store accel X, Y,
- *              and Z axis bias.
- *  @param  perform_full_test
- *              If 1:
- *              calculates offsets and noise and compare it against set
- *              thresholds. The final exist status will reflect if any of the
- *              value is outside of the expected range.
- *              When 0;
- *              skip the noise calculation and pass/fail assessment; simply
- *              calculates the accel biases.
- *
- *  @return 0 on success. A non-zero error code on error.
- */
-int inv_test_accel(void *mlsl_handle,
-                   short *bias, long gravity,
-                   uint_fast8_t perform_full_test)
-{
-    int i;
-
-    short *p_vals;
-    float x = 0.f, y = 0.f, z = 0.f, zg = 0.f;
-    float RMS[3];
-    float accelRmsThresh = 1000000.f; /* enourmous so that the test always
-                                         passes - future deployment */
-    unsigned short res;
-    unsigned long orig_requested_sensors;
-    struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata;
-
-    p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
-
-    /* load the slave descr from the getter */
-    if (mputestPData->accel.get_slave_descr == NULL) {
-        MPL_LOGI("\n");
-        MPL_LOGI("No accelerometer configured\n");
-        return 0;
-    }
-    if (mputestCfgPtr->accel == NULL) {
-        MPL_LOGI("\n");
-        MPL_LOGI("No accelerometer configured\n");
-        return 0;
-    }
-
-    /* resume the accel */
-    orig_requested_sensors = mputestCfgPtr->requested_sensors;
-    mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO;
-    res = inv_mpu_resume(mputestCfgPtr,
-                         mlsl_handle, NULL, NULL, NULL,
-                         mputestCfgPtr->requested_sensors);
-    if(res != INV_SUCCESS)
-        goto accel_error;
-
-    /* wait at least a sample cycle for the
-       accel data to be retrieved by MPU */
-    inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000);
-
-    /* collect the samples  */
-    for(i = 0; i < test_setup.accel_samples; i++) {
-        short *vals = &p_vals[3 * i];
-        if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) {
-            goto accel_error;
-        }
-        x += 1.f * vals[X] / test_setup.accel_samples;
-        y += 1.f * vals[Y] / test_setup.accel_samples;
-        z += 1.f * vals[Z] / test_setup.accel_samples;
-    }
-
-    mputestCfgPtr->requested_sensors = orig_requested_sensors;
-    res = inv_mpu_suspend(mputestCfgPtr,
-                          mlsl_handle, NULL, NULL, NULL,
-                          INV_ALL_SENSORS);
-    if (res != INV_SUCCESS)
-        goto accel_error;
-
-    MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z);
-    if (VERBOSE_OUT) {
-        MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (gee)\n",
-                    x / gravity, y / gravity, z / gravity);
-    }
-
-    bias[0] = FLOAT_TO_SHORT(x);
-    bias[1] = FLOAT_TO_SHORT(y);
-    zg = z - g_z_sign * gravity;
-    bias[2] = FLOAT_TO_SHORT(zg);
-
-    MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
-             bias[0], bias[1], bias[2]);
-    if (VERBOSE_OUT) {
-        MPL_LOGI("Accel correct.: "
-               "%+13.3f %+13.3f %+13.3f (gee)\n",
-                    1.f * bias[0] / gravity,
-                    1.f * bias[1] / gravity,
-                    1.f * bias[2] / gravity);
-    }
-
-    if (perform_full_test) {
-        /* accel RMS - for now the threshold is only indicative */
-        for (i = 0,
-                 RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
-             i <  test_setup.accel_samples; i++) {
-            short *vals = &p_vals[3 * i];
-            RMS[X] += (vals[X] - x) * (vals[X] - x);
-            RMS[Y] += (vals[Y] - y) * (vals[Y] - y);
-            RMS[Z] += (vals[Z] - z) * (vals[Z] - z);
-        }
-        for (i = 0; i < 3; i++) {
-            if (RMS[i] >  accelRmsThresh * accelRmsThresh
-                            * test_setup.accel_samples) {
-                MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
-                         "(threshold = %.2f)\n",
-                         a_name[i], sqrt(RMS[i] / test_setup.accel_samples),
-                         accelRmsThresh);
-                goto accel_error;
-            }
-        }
-        MPL_LOGI("RMS           : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
-                 sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES),
-                 sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES),
-                 sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES));
-    }
-
-    return 0; /* success */
-
-accel_error:  /* error */
-    bias[0] = bias[1] = bias[2] = 0;
-    return 1;
-}
-
-/**
- *  @brief  an user-space substitute of the power management function(s)
- *          in mldl_cfg.c for self test usage.
- *          Wake up and sleep the device, whether that is MPU3050, MPU6050A2,
- *          or MPU6050B1.
- *  @param  mlsl_handle
- *              a file handle for the serial communication port used to
- *              communicate with the MPU device.
- *  @param  power_level
- *              the power state to change the device into. Currently only 0 or
- *              1 are supported, for sleep and full-power respectively.
- *  @return 0 on success; a non-zero error code on error.
- */
-static inv_error_t inv_device_power_mgmt(void *mlsl_handle,
-                                         uint_fast8_t power_level)
-{
-    inv_error_t result;
-    static unsigned char pwr_mgm;
-    unsigned char b;
-
-    if (power_level != 0 && power_level != 1) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    if (power_level) {
-        result = inv_serial_read(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGM, 1, &pwr_mgm);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        /* reset */
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET);
-#ifndef CONFIG_MPU_SENSORS_MPU6050A2
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-#endif
-        inv_sleep(5);
-
-        /* re-read power mgmt reg -
-           it may have reset after H_RESET is applied */
-        result = inv_serial_read(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_PWR_MGM, 1, &b);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-
-        /* wake up */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
-        if ((b & BITS_PWRSEL) != BITS_PWRSEL) {
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_PWR_MGM, BITS_PWRSEL);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-#else
-        if (pwr_mgm & BIT_SLEEP) {
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_PWR_MGM, 0x00);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-#endif
-        inv_sleep(60);
-
-#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \
-    defined(CONFIG_MPU_SENSORS_MPU6050B1)
-        result = inv_serial_single_write(
-                    mlsl_handle, mputestCfgPtr->addr,
-                    MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-#endif
-    } else {
-        /* restore the power state the part was found in */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
-        if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) {
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_PWR_MGM, pwr_mgm);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-#else
-        if (pwr_mgm & BIT_SLEEP) {
-            result = inv_serial_single_write(
-                        mlsl_handle, mputestCfgPtr->addr,
-                        MPUREG_PWR_MGM, pwr_mgm);
-            if (result) {
-                LOG_RESULT_LOCATION(result);
-                return result;
-            }
-        }
-#endif
-    }
-
-    return INV_SUCCESS;
-}
-
-/**
- *  @brief  The main entry point of the MPU Self Test, triggering the run of
- *          the single tests, for gyros and accelerometers.
- *          Prepares the MPU for the test, taking the device out of low power
- *          state if necessary, switching the MPU secondary I2C interface into
- *          bypass mode and restoring the original power state at the end of
- *          the test.
- *          This function is also responsible for encoding the output of each
- *          test in the correct format as it is stored on the file/medium of
- *          choice (according to inv_serial_write_cal() function).
- *          The format needs to stay perfectly consistent with the one expected
- *          by the corresponding loader in ml_stored_data.c; currectly the
- *          loaded in use is inv_load_cal_V1 (record type 1 - initial
- *          calibration).
- *
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  provide_result
- *              If 1:
- *              perform and analyze the offset, drive frequency, and noise
- *              calculation and compare it against set threshouds. Report
- *              also the final result using a bit-mask like error code as
- *              described in the inv_test_gyro() function.
- *              When 0:
- *              skip the noise and drive frequency calculation and pass/fail
- *              assessment. It simply calculates the gyro and accel biases.
- *              NOTE: for MPU6050 devices, this parameter is currently
- *              ignored.
- *
- *  @return 0 on success.  A non-zero error code on error.
- *          Propagates the errors from the tests up to the caller.
- */
-int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result)
-{
-    int result = 0;
-
-    short temp_avg;
-    short gyro_biases[3] = {0, 0, 0};
-    short accel_biases[3] = {0, 0, 0};
-
-    unsigned long testStart = inv_get_tick_count();
-    long accelSens[3] = {0};
-    int ptr;
-    int tmp;
-    long long lltmp;
-    uint32_t chk;
-
-    MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n",
-                DEF_TEST_TIME_PER_AXIS / 600);
-    MPL_LOGI("\n");
-
-    result = inv_device_power_mgmt(mlsl_handle, TRUE);
-
-#ifdef TRACK_IDS
-    result = test_get_mpu_id(mlsl_handle);
-    if (result != INV_SUCCESS) {
-        MPL_LOGI("Could not read the device's unique ID\n");
-        MPL_LOGI("\n");
-        return result;
-    }
-#endif
-
-    /* adjust the gyro sensitivity according to the gyro_sens_trim value */
-    adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f;
-    test_setup.gyro_fs = (int)(32768.f / adjGyroSens);
-
-    /* collect gyro and temperature data */
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-    result = inv_test_gyro_3050(mlsl_handle,
-                gyro_biases, &temp_avg, provide_result);
-#else
-    MPL_LOGW_IF(provide_result,
-                "Self Test for MPU-6050 devices is for bias correction only: "
-                "no test PASS/FAIL result will be provided\n");
-    result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg);
-#endif
-
-    MPL_LOGI("\n");
-    MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart);
-    if (result)
-        return result;
-
-    /* collect accel data.  if this step is skipped,
-       ensure the array still contains zeros. */
-    if (mputestCfgPtr->accel != NULL) {
-        float fs;
-        RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs);
-        accelSens[0] = (long)(32768L / fs);
-        accelSens[1] = (long)(32768L / fs);
-        accelSens[2] = (long)(32768L / fs);
-#if defined CONFIG_MPU_SENSORS_MPU6050B1
-        if (MPL_PROD_KEY(mputestCfgPtr->product_id,
-                mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) {
-            accelSens[2] /= 2;
-        } else {
-            unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim;
-            accelSens[0] /= trim_corr;
-            accelSens[1] /= trim_corr;
-            accelSens[2] /= trim_corr;
-        }
-#endif
-    } else {
-        /* would be 0, but 1 to avoid divide-by-0 below */
-        accelSens[0] = accelSens[1] = accelSens[2] = 1;
-    }
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-    result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
-                            provide_result);
-#else
-    result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
-                            FALSE);
-#endif
-    if (result)
-        return result;
-
-    result = inv_device_power_mgmt(mlsl_handle, FALSE);
-    if (result)
-        return result;
-
-    ptr = 0;
-    dataStore[ptr++] = 0;       /* total len of factory cal */
-    dataStore[ptr++] = 0;
-    dataStore[ptr++] = 0;
-    dataStore[ptr++] = ML_INIT_CAL_LEN;
-    dataStore[ptr++] = 0;
-    dataStore[ptr++] = 5;       /* record type 5 - initial calibration */
-
-    tmp = temp_avg;             /* temperature */
-    if (tmp < 0) tmp += 2 << 16;
-    USHORT_TO_CHARS(&dataStore[ptr], tmp);
-    ptr += 2;
-
-    /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
-    lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-    lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-    lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-
-    lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-    lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-    lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */
-    if (lltmp < 0) lltmp += 1LL << 32;
-    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
-    ptr += 4;
-
-    /* add a checksum for data */
-    chk = inv_checksum(
-        dataStore + INV_CAL_HDR_LEN,
-        ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
-    UINT_TO_CHARS(&dataStore[ptr], chk);
-    ptr += 4;
-
-    if (ptr != ML_INIT_CAL_LEN) {
-        MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
-                    ML_INIT_CAL_LEN, ptr);
-        return -1;
-    }
-
-    return result;
-}
-
-/**
- *  @brief  The main test API. Runs the MPU Self Test and, if successful,
- *          stores the encoded initial calibration data on the final storage
- *          medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE
- *          define in your mlsl implementation).
- *
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  provide_result
- *              If 1:
- *              perform and analyze the offset, drive frequency, and noise
- *              calculation and compare it against set threshouds. Report
- *              also the final result using a bit-mask like error code as
- *              described in the inv_test_gyro() function.
- *              When 0:
- *              skip the noise and drive frequency calculation and pass/fail
- *              assessment. It simply calculates the gyro and accel biases.
- *
- *  @return 0 on success or a non-zero error code from the callees on error.
- */
-inv_error_t inv_factory_calibrate(void *mlsl_handle,
-                                  uint_fast8_t provide_result)
-{
-    int result;
-
-    result = inv_mpu_test(mlsl_handle, provide_result);
-    if (provide_result) {
-        MPL_LOGI("\n");
-        if (result == 0) {
-            MPL_LOGI("Test : PASSED\n");
-        } else {
-            MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result);
-            return result; /* abort writing the calibration if the
-                              test is not successful */
-        }
-        MPL_LOGI("\n");
-    } else {
-        MPL_LOGI("\n");
-        if (result) {
-            LOG_RESULT_LOCATION(result);
-            return result;
-        }
-    }
-
-    result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN);
-    if (result) {
-        MPL_LOGI("Error : cannot write calibration on file - error %d\n",
-            result);
-        return result;
-    }
-
-    return INV_SUCCESS;
-}
-
-
-
-/* -----------------------------------------------------------------------
-    accel interface functions
- -----------------------------------------------------------------------*/
-
-/**
- *  @internal
- *  @brief  Reads data for X, Y, and Z axis from the accelerometer device.
- *          Used only if an accelerometer has been setup using the
- *          inv_test_setup_accel() API.
- *          Takes care of the accelerometer endianess according to how the
- *          device has been described in the corresponding accelerometer driver
- *          file.
- *  @param  mlsl_handle
- *              serial interface handle to allow serial communication with the
- *              device, both gyro and accelerometer.
- *  @param  slave
- *              a pointer to the descriptor of the slave accelerometer device
- *              in use. Contains the necessary information to operate, read,
- *              and communicate with the accelerometer device of choice.
- *              See the declaration of struct ext_slave_descr in mpu.h.
- *  @param  pdata
- *              a pointer to the platform info of the slave accelerometer
- *              device in use. Describes how the device is oriented and
- *              mounted on host platform's PCB.
- *  @param  vals
- *              output pointer to return the accelerometer's X, Y, and Z axis
- *              sensor data collected.
- *  @return 0 on success or a non-zero error code on error.
- */
-static inv_error_t test_get_data(
-                    void *mlsl_handle,
-                    struct mldl_cfg *mputestCfgPtr,
-                    short *vals)
-{
-    inv_error_t result;
-    unsigned char data[20];
-    struct ext_slave_descr *slave = mputestCfgPtr->accel;
-#ifndef CONFIG_MPU_SENSORS_MPU3050
-    struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel;
-#endif
-
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-    result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23,
-                             6, data);
-#else
-    result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg,
-                            slave->read_len, data);
-#endif
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    }
-
-    if (VERBOSE_OUT) {
-        MPL_LOGI("Accel         :        0x%02X%02X        0x%02X%02X        0x%02X%02X (raw)\n",
-            ACCEL_UNPACK(data));
-    }
-
-    if (CHECK_NACKS(data)) {
-        MPL_LOGI("Error fetching data from the accelerometer : "
-                 "all bytes read 0xff\n");
-        return INV_ERROR_SERIAL_READ;
-    }
-
-    if (slave->endian == EXT_SLAVE_BIG_ENDIAN) {
-        vals[0] = CHARS_TO_SHORT(&data[0]);
-        vals[1] = CHARS_TO_SHORT(&data[2]);
-        vals[2] = CHARS_TO_SHORT(&data[4]);
-    } else {
-        vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]);
-        vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]);
-        vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]);
-    }
-
-    if (VERBOSE_OUT) {
-        MPL_LOGI("Accel         : %+13d %+13d %+13d (LSB)\n",
-                 vals[0], vals[1], vals[2]);
-    }
-    return INV_SUCCESS;
-}
-
-#ifdef __cplusplus
-}
-#endif
-
-/**
- *  @}
- */
-
diff --git a/mlsdk/mlutils/mputest.h b/mlsdk/mlutils/mputest.h
deleted file mode 100644
index d3347c5..0000000
--- a/mlsdk/mlutils/mputest.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- *
- * $Id: mputest.h 4051 2010-11-19 04:51:58Z mcaramello $
- *
- *****************************************************************************/
-
-#ifndef MPUTEST_H
-#define MPUTEST_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mlsl.h"
-#include "mldl_cfg.h"
-#include "mputest_legacy.h"
-
-/* user facing APIs */
-inv_error_t inv_factory_calibrate(void *mlsl_handle,
-                                  uint_fast8_t provide_result);
-void inv_set_test_parameters(unsigned int slave_addr, float sensitivity,
-                             int p_thresh, float total_time_tol,
-                             int bias_thresh, float rms_thresh,
-                             float sp_shift_thresh,
-                             unsigned short accel_samples);
-
-/* additional functions */
-int  inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result);
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* MPUTEST_H */
-
diff --git a/mlsdk/platform/include/i2c.h b/mlsdk/platform/include/i2c.h
deleted file mode 100644
index 39dd255..0000000
--- a/mlsdk/platform/include/i2c.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: i2c.h 5629 2011-06-11 03:13:08Z mcaramello $
- * 
- *******************************************************************************/
-
-#ifndef _I2C_H
-#define _I2C_H
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-
-/* - Error Codes. - */
-
-#define I2C_SUCCESS 0
-#define I2C_ERROR   1
-
-/* ---------- */
-/* - Enums. - */
-/* ---------- */
-
-/* --------------- */
-/* - Structures. - */
-/* --------------- */
-
-#define I2C_RBUFF_MAX    128
-#define I2C_RBUFF_SIZE    17
-
-#ifdef BB_I2C
-#define I2C_RBUFF_DYNAMIC  0
-#else
-#define I2C_RBUFF_DYNAMIC  1
-#endif
-
-typedef struct {
-
-	HANDLE  i2cHndl;
-	HANDLE  hDevice;                    // handle to the drive to be examined 
-
-    MLU8      readBuffer[1024];
-	MLU8      writeBuffer[1024];
-
-    MLU16     rBuffRIndex;
-	MLU16     rBuffWIndex;
-#if !I2C_RBUFF_DYNAMIC 
-    MLU8      rBuff[I2C_RBUFF_MAX][I2C_RBUFF_SIZE];
-#else
-    MLU8     *rBuff;
-#endif
-    MLU16     rBuffMax;
-	MLU16     rBuffNumBytes;
-
-    MLU8      runThread;
-	MLU8      autoProcess;
-
-} I2C_Vars_t;
-
-/* --------------------- */
-/* - Function p-types. - */
-/* --------------------- */
-
-#if (defined(BB_I2C))
-void set_i2c_open_bind_cb(int (*func)(unsigned int i2c_slave_addr));
-void set_i2c_open_cb(int (*func)(const char *dev, int rw));
-void set_i2c_close_cb(int (*func)(int fd));
-void set_i2c_lltransfer_cb(int (*func)(int fd, int client_addr, const char *write_buf, unsigned int write_len,
-                                                                      char *read_buf,  unsigned int read_len ));
-void set_i2c_write_register_cb(int (*func)(int fd, int client_addr, unsigned char reg, unsigned char value));
-void set_i2c_read_register_cb(unsigned char (*func)(int fd, int client_addr, unsigned char reg));
-void set_i2c_dump_register_cb(int (*func)(int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len));
-
-int           _i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value);
-unsigned char _i2c_read_register (int fd, int client_addr, unsigned char reg);
-int            i2c_lltransfer    (int fd, int client_addr, const char *write_buf, unsigned int write_len,
-                                                                 char *read_buf,  unsigned int read_len );
-int            i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value);
-unsigned char  i2c_read_register (int fd, int client_addr, unsigned char reg);
-int            i2c_dump_register (int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len);
-int i2c_open         (const char *dev, int rw);
-int i2c_close        (int fd);
-int i2c_open_bind    (unsigned int i2c_slave_addr);
-#endif
-
-int I2COpen           (unsigned char autoProcess, unsigned char createThread);
-int I2CClose          (void);
-int I2CDeviceIoControl(void);
-int I2CRead           (void);
-int I2CWrite          (void);
-int I2CSetBufferSize  (unsigned short bufferSize);
-int I2CBufferUpdate   (void);
-int I2CHandler        (void);
-int I2CReadBuffer     (unsigned short cnt, unsigned char bufferMode, unsigned char *rBuff);
-int I2CEmptyBuffer    (void);
-int I2CPktsInBuffer   (unsigned short *pktsInBuffer);
-int I2CCreateMutex    (void);
-int I2CLockMutex      (void);
-int I2CUnlockMutex    (void);
-
-int I2CWriteBurst     (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data);
-int I2CReadBurst      (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data);
-
-int I2COpenBB         (void);
-int I2CCloseBB        (int i2cHandle);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _TEMPLATE_H */
diff --git a/mlsdk/platform/include/mltypes.h b/mlsdk/platform/include/mltypes.h
deleted file mode 100644
index 90a126b..0000000
--- a/mlsdk/platform/include/mltypes.h
+++ /dev/null
@@ -1,265 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/**
- *  @defgroup MLERROR
- *  @brief  Motion Library - Error definitions.
- *          Definition of the error codes used within the MPL and
- *          returned to the user.
- *          Every function tries to return a meaningful error code basing
- *          on the occuring error condition. The error code is numeric.
- *
- *          The available error codes and their associated values are:
- *          - (0)       INV_SUCCESS
- *          - (1)       INV_ERROR
- *          - (2)       INV_ERROR_INVALID_PARAMETER
- *          - (3)       INV_ERROR_FEATURE_NOT_ENABLED
- *          - (4)       INV_ERROR_FEATURE_NOT_IMPLEMENTED
- *          - (6)       INV_ERROR_DMP_NOT_STARTED
- *          - (7)       INV_ERROR_DMP_STARTED
- *          - (8)       INV_ERROR_NOT_OPENED
- *          - (9)       INV_ERROR_OPENED
- *          - (10)      INV_ERROR_INVALID_MODULE
- *          - (11)      INV_ERROR_MEMORY_EXAUSTED
- *          - (12)      INV_ERROR_DIVIDE_BY_ZERO
- *          - (13)      INV_ERROR_ASSERTION_FAILURE
- *          - (14)      INV_ERROR_FILE_OPEN
- *          - (15)      INV_ERROR_FILE_READ
- *          - (16)      INV_ERROR_FILE_WRITE
- *          - (17)      INV_ERROR_INVALID_CONFIGURATION
- *          - (20)      INV_ERROR_SERIAL_CLOSED
- *          - (21)      INV_ERROR_SERIAL_OPEN_ERROR
- *          - (22)      INV_ERROR_SERIAL_READ
- *          - (23)      INV_ERROR_SERIAL_WRITE
- *          - (24)      INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
- *          - (25)      INV_ERROR_SM_TRANSITION
- *          - (26)      INV_ERROR_SM_IMPROPER_STATE
- *          - (30)      INV_ERROR_FIFO_OVERFLOW
- *          - (31)      INV_ERROR_FIFO_FOOTER
- *          - (32)      INV_ERROR_FIFO_READ_COUNT
- *          - (33)      INV_ERROR_FIFO_READ_DATA
- *          - (40)      INV_ERROR_MEMORY_SET
- *          - (50)      INV_ERROR_LOG_MEMORY_ERROR
- *          - (51)      INV_ERROR_LOG_OUTPUT_ERROR
- *          - (60)      INV_ERROR_OS_BAD_PTR
- *          - (61)      INV_ERROR_OS_BAD_HANDLE
- *          - (62)      INV_ERROR_OS_CREATE_FAILED
- *          - (63)      INV_ERROR_OS_LOCK_FAILED
- *          - (70)      INV_ERROR_COMPASS_DATA_OVERFLOW
- *          - (71)      INV_ERROR_COMPASS_DATA_UNDERFLOW
- *          - (72)      INV_ERROR_COMPASS_DATA_NOT_READY
- *          - (73)      INV_ERROR_COMPASS_DATA_ERROR
- *          - (75)      INV_ERROR_CALIBRATION_LOAD
- *          - (76)      INV_ERROR_CALIBRATION_STORE
- *          - (77)      INV_ERROR_CALIBRATION_LEN
- *          - (78)      INV_ERROR_CALIBRATION_CHECKSUM
- *          - (79)      INV_ERROR_ACCEL_DATA_OVERFLOW
- *          - (80)      INV_ERROR_ACCEL_DATA_UNDERFLOW
- *          - (81)      INV_ERROR_ACCEL_DATA_NOT_READY
- *          - (82)      INV_ERROR_ACCEL_DATA_ERROR
- *
- *  @{
- *      @file mltypes.h
- *  @}
- */
-
-#ifndef MLTYPES_H
-#define MLTYPES_H
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include "stdint_invensense.h"
-#endif
-
-/*---------------------------
-    ML Types
----------------------------*/
-
-/**
- *  @struct inv_error_t mltypes.h "mltypes"
- *  @brief  The MPL Error Code return type.
- *
- *  @code
- *      typedef unsigned char inv_error_t;
- *  @endcode
- */
-typedef unsigned char inv_error_t;
-
-#ifndef __cplusplus
-#ifndef __KERNEL__
-typedef int_fast8_t bool;
-#endif
-#endif
-
-/*---------------------------
-    ML Defines
----------------------------*/
-
-#ifndef NULL
-#define NULL 0
-#endif
-
-#ifndef TRUE
-#define TRUE 1
-#endif
-
-#ifndef FALSE
-#define FALSE 0
-#endif
-
-#ifndef __KERNEL__
-#ifndef ARRAY_SIZE
-/* Dimension of an array */
-#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
-#endif
-#endif
-/* - ML Errors. - */
-#define ERROR_NAME(x)   (#x)
-#define ERROR_CHECK_FIRST(first, x)                                     \
-	{ if (INV_SUCCESS == first) first = x; }
-
-#define INV_SUCCESS                       (0)
-/* Generic Error code.  Proprietary Error Codes only */
-#define INV_ERROR                         (1)
-
-/* Compatibility and other generic error codes */
-#define INV_ERROR_INVALID_PARAMETER       (2)
-#define INV_ERROR_FEATURE_NOT_ENABLED     (3)
-#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (4)
-#define INV_ERROR_DMP_NOT_STARTED         (6)
-#define INV_ERROR_DMP_STARTED             (7)
-#define INV_ERROR_NOT_OPENED              (8)
-#define INV_ERROR_OPENED                  (9)
-#define INV_ERROR_INVALID_MODULE         (10)
-#define INV_ERROR_MEMORY_EXAUSTED        (11)
-#define INV_ERROR_DIVIDE_BY_ZERO         (12)
-#define INV_ERROR_ASSERTION_FAILURE      (13)
-#define INV_ERROR_FILE_OPEN              (14)
-#define INV_ERROR_FILE_READ              (15)
-#define INV_ERROR_FILE_WRITE             (16)
-#define INV_ERROR_INVALID_CONFIGURATION  (17)
-
-/* Serial Communication */
-#define INV_ERROR_SERIAL_CLOSED          (20)
-#define INV_ERROR_SERIAL_OPEN_ERROR      (21)
-#define INV_ERROR_SERIAL_READ            (22)
-#define INV_ERROR_SERIAL_WRITE           (23)
-#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (24)
-
-/* SM = State Machine */
-#define INV_ERROR_SM_TRANSITION          (25)
-#define INV_ERROR_SM_IMPROPER_STATE      (26)
-
-/* Fifo */
-#define INV_ERROR_FIFO_OVERFLOW          (30)
-#define INV_ERROR_FIFO_FOOTER            (31)
-#define INV_ERROR_FIFO_READ_COUNT        (32)
-#define INV_ERROR_FIFO_READ_DATA         (33)
-
-/* Memory & Registers, Set & Get */
-#define INV_ERROR_MEMORY_SET             (40)
-
-#define INV_ERROR_LOG_MEMORY_ERROR       (50)
-#define INV_ERROR_LOG_OUTPUT_ERROR       (51)
-
-/* OS interface errors */
-#define INV_ERROR_OS_BAD_PTR             (60)
-#define INV_ERROR_OS_BAD_HANDLE          (61)
-#define INV_ERROR_OS_CREATE_FAILED       (62)
-#define INV_ERROR_OS_LOCK_FAILED         (63)
-
-/* Compass errors */
-#define INV_ERROR_COMPASS_DATA_OVERFLOW  (70)
-#define INV_ERROR_COMPASS_DATA_UNDERFLOW (71)
-#define INV_ERROR_COMPASS_DATA_NOT_READY (72)
-#define INV_ERROR_COMPASS_DATA_ERROR     (73)
-
-/* Load/Store calibration */
-#define INV_ERROR_CALIBRATION_LOAD       (75)
-#define INV_ERROR_CALIBRATION_STORE      (76)
-#define INV_ERROR_CALIBRATION_LEN        (77)
-#define INV_ERROR_CALIBRATION_CHECKSUM   (78)
-
-/* Accel errors */
-#define INV_ERROR_ACCEL_DATA_OVERFLOW    (79)
-#define INV_ERROR_ACCEL_DATA_UNDERFLOW   (80)
-#define INV_ERROR_ACCEL_DATA_NOT_READY   (81)
-#define INV_ERROR_ACCEL_DATA_ERROR       (82)
-
-#ifdef INV_USE_LEGACY_NAMES
-#define ML_SUCCESS                       INV_SUCCESS
-#define ML_ERROR                         INV_ERROR
-#define ML_ERROR_INVALID_PARAMETER       INV_ERROR_INVALID_PARAMETER
-#define ML_ERROR_FEATURE_NOT_ENABLED     INV_ERROR_FEATURE_NOT_ENABLED
-#define ML_ERROR_FEATURE_NOT_IMPLEMENTED INV_ERROR_FEATURE_NOT_IMPLEMENTED
-#define ML_ERROR_DMP_NOT_STARTED         INV_ERROR_DMP_NOT_STARTED
-#define ML_ERROR_DMP_STARTED             INV_ERROR_DMP_STARTED
-#define ML_ERROR_NOT_OPENED              INV_ERROR_NOT_OPENED
-#define ML_ERROR_OPENED                  INV_ERROR_OPENED
-#define ML_ERROR_INVALID_MODULE          INV_ERROR_INVALID_MODULE
-#define ML_ERROR_MEMORY_EXAUSTED         INV_ERROR_MEMORY_EXAUSTED
-#define ML_ERROR_DIVIDE_BY_ZERO          INV_ERROR_DIVIDE_BY_ZERO
-#define ML_ERROR_ASSERTION_FAILURE       INV_ERROR_ASSERTION_FAILURE
-#define ML_ERROR_FILE_OPEN               INV_ERROR_FILE_OPEN
-#define ML_ERROR_FILE_READ               INV_ERROR_FILE_READ
-#define ML_ERROR_FILE_WRITE              INV_ERROR_FILE_WRITE
-#define ML_ERROR_INVALID_CONFIGURATION   INV_ERROR_INVALID_CONFIGURATION
-#define ML_ERROR_SERIAL_CLOSED           INV_ERROR_SERIAL_CLOSED
-#define ML_ERROR_SERIAL_OPEN_ERROR       INV_ERROR_SERIAL_OPEN_ERROR
-#define ML_ERROR_SERIAL_READ             INV_ERROR_SERIAL_READ
-#define ML_ERROR_SERIAL_WRITE            INV_ERROR_SERIAL_WRITE
-#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  \
-	INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
-#define ML_ERROR_SM_TRANSITION          INV_ERROR_SM_TRANSITION
-#define ML_ERROR_SM_IMPROPER_STATE      INV_ERROR_SM_IMPROPER_STATE
-#define ML_ERROR_FIFO_OVERFLOW          INV_ERROR_FIFO_OVERFLOW
-#define ML_ERROR_FIFO_FOOTER            INV_ERROR_FIFO_FOOTER
-#define ML_ERROR_FIFO_READ_COUNT        INV_ERROR_FIFO_READ_COUNT
-#define ML_ERROR_FIFO_READ_DATA         INV_ERROR_FIFO_READ_DATA
-#define ML_ERROR_MEMORY_SET             INV_ERROR_MEMORY_SET
-#define ML_ERROR_LOG_MEMORY_ERROR       INV_ERROR_LOG_MEMORY_ERROR
-#define ML_ERROR_LOG_OUTPUT_ERROR       INV_ERROR_LOG_OUTPUT_ERROR
-#define ML_ERROR_OS_BAD_PTR             INV_ERROR_OS_BAD_PTR
-#define ML_ERROR_OS_BAD_HANDLE          INV_ERROR_OS_BAD_HANDLE
-#define ML_ERROR_OS_CREATE_FAILED       INV_ERROR_OS_CREATE_FAILED
-#define ML_ERROR_OS_LOCK_FAILED         INV_ERROR_OS_LOCK_FAILED
-#define ML_ERROR_COMPASS_DATA_OVERFLOW  INV_ERROR_COMPASS_DATA_OVERFLOW
-#define ML_ERROR_COMPASS_DATA_UNDERFLOW INV_ERROR_COMPASS_DATA_UNDERFLOW
-#define ML_ERROR_COMPASS_DATA_NOT_READY INV_ERROR_COMPASS_DATA_NOT_READY
-#define ML_ERROR_COMPASS_DATA_ERROR     INV_ERROR_COMPASS_DATA_ERROR
-#define ML_ERROR_CALIBRATION_LOAD       INV_ERROR_CALIBRATION_LOAD
-#define ML_ERROR_CALIBRATION_STORE      INV_ERROR_CALIBRATION_STORE
-#define ML_ERROR_CALIBRATION_LEN        INV_ERROR_CALIBRATION_LEN
-#define ML_ERROR_CALIBRATION_CHECKSUM   INV_ERROR_CALIBRATION_CHECKSUM
-#define ML_ERROR_ACCEL_DATA_OVERFLOW    INV_ERROR_ACCEL_DATA_OVERFLOW
-#define ML_ERROR_ACCEL_DATA_UNDERFLOW   INV_ERROR_ACCEL_DATA_UNDERFLOW
-#define ML_ERROR_ACCEL_DATA_NOT_READY   INV_ERROR_ACCEL_DATA_NOT_READY
-#define ML_ERROR_ACCEL_DATA_ERROR       INV_ERROR_ACCEL_DATA_ERROR
-#endif
-
-/* For Linux coding compliance */
-#ifndef __KERNEL__
-#define EXPORT_SYMBOL(x)
-#endif
-
-/*---------------------------
-    p-Types
----------------------------*/
-
-#endif				/* MLTYPES_H */
diff --git a/mlsdk/platform/include/mpu3050.h b/mlsdk/platform/include/mpu3050.h
deleted file mode 100644
index c363a00..0000000
--- a/mlsdk/platform/include/mpu3050.h
+++ /dev/null
@@ -1,255 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 __MPU_H_
-#error Do not include this file directly.  Include mpu.h instead.
-#endif
-
-#ifndef __MPU3050_H_
-#define __MPU3050_H_
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#endif
-
-#if !defined CONFIG_MPU_SENSORS_MPU3050
-#error MPU6000 build including MPU3050 header
-#endif
-
-#define MPU_NAME "mpu3050"
-#define DEFAULT_MPU_SLAVEADDR       0x68
-
-/*==== MPU REGISTER SET ====*/
-enum mpu_register {
-	MPUREG_WHO_AM_I = 0,	/* 00 0x00 */
-	MPUREG_PRODUCT_ID,	/* 01 0x01 */
-	MPUREG_02_RSVD,		/* 02 0x02 */
-	MPUREG_03_RSVD,		/* 03 0x03 */
-	MPUREG_04_RSVD,		/* 04 0x04 */
-	MPUREG_XG_OFFS_TC,	/* 05 0x05 */
-	MPUREG_06_RSVD,		/* 06 0x06 */
-	MPUREG_07_RSVD,		/* 07 0x07 */
-	MPUREG_YG_OFFS_TC,	/* 08 0x08 */
-	MPUREG_09_RSVD,		/* 09 0x09 */
-	MPUREG_0A_RSVD,		/* 10 0x0a */
-	MPUREG_ZG_OFFS_TC,	/* 11 0x0b */
-	MPUREG_X_OFFS_USRH,	/* 12 0x0c */
-	MPUREG_X_OFFS_USRL,	/* 13 0x0d */
-	MPUREG_Y_OFFS_USRH,	/* 14 0x0e */
-	MPUREG_Y_OFFS_USRL,	/* 15 0x0f */
-	MPUREG_Z_OFFS_USRH,	/* 16 0x10 */
-	MPUREG_Z_OFFS_USRL,	/* 17 0x11 */
-	MPUREG_FIFO_EN1,	/* 18 0x12 */
-	MPUREG_FIFO_EN2,	/* 19 0x13 */
-	MPUREG_AUX_SLV_ADDR,	/* 20 0x14 */
-	MPUREG_SMPLRT_DIV,	/* 21 0x15 */
-	MPUREG_DLPF_FS_SYNC,	/* 22 0x16 */
-	MPUREG_INT_CFG,		/* 23 0x17 */
-	MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */
-	MPUREG_19_RSVD,		/* 25 0x19 */
-	MPUREG_INT_STATUS,	/* 26 0x1a */
-	MPUREG_TEMP_OUT_H,	/* 27 0x1b */
-	MPUREG_TEMP_OUT_L,	/* 28 0x1c */
-	MPUREG_GYRO_XOUT_H,	/* 29 0x1d */
-	MPUREG_GYRO_XOUT_L,	/* 30 0x1e */
-	MPUREG_GYRO_YOUT_H,	/* 31 0x1f */
-	MPUREG_GYRO_YOUT_L,	/* 32 0x20 */
-	MPUREG_GYRO_ZOUT_H,	/* 33 0x21 */
-	MPUREG_GYRO_ZOUT_L,	/* 34 0x22 */
-	MPUREG_23_RSVD,		/* 35 0x23 */
-	MPUREG_24_RSVD,		/* 36 0x24 */
-	MPUREG_25_RSVD,		/* 37 0x25 */
-	MPUREG_26_RSVD,		/* 38 0x26 */
-	MPUREG_27_RSVD,		/* 39 0x27 */
-	MPUREG_28_RSVD,		/* 40 0x28 */
-	MPUREG_29_RSVD,		/* 41 0x29 */
-	MPUREG_2A_RSVD,		/* 42 0x2a */
-	MPUREG_2B_RSVD,		/* 43 0x2b */
-	MPUREG_2C_RSVD,		/* 44 0x2c */
-	MPUREG_2D_RSVD,		/* 45 0x2d */
-	MPUREG_2E_RSVD,		/* 46 0x2e */
-	MPUREG_2F_RSVD,		/* 47 0x2f */
-	MPUREG_30_RSVD,		/* 48 0x30 */
-	MPUREG_31_RSVD,		/* 49 0x31 */
-	MPUREG_32_RSVD,		/* 50 0x32 */
-	MPUREG_33_RSVD,		/* 51 0x33 */
-	MPUREG_34_RSVD,		/* 52 0x34 */
-	MPUREG_DMP_CFG_1,	/* 53 0x35 */
-	MPUREG_DMP_CFG_2,	/* 54 0x36 */
-	MPUREG_BANK_SEL,	/* 55 0x37 */
-	MPUREG_MEM_START_ADDR,	/* 56 0x38 */
-	MPUREG_MEM_R_W,		/* 57 0x39 */
-	MPUREG_FIFO_COUNTH,	/* 58 0x3a */
-	MPUREG_FIFO_COUNTL,	/* 59 0x3b */
-	MPUREG_FIFO_R_W,	/* 60 0x3c */
-	MPUREG_USER_CTRL,	/* 61 0x3d */
-	MPUREG_PWR_MGM,		/* 62 0x3e */
-	MPUREG_3F_RSVD,		/* 63 0x3f */
-	NUM_OF_MPU_REGISTERS	/* 64 0x40 */
-};
-
-/*==== BITS FOR MPU ====*/
-
-/*---- MPU 'FIFO_EN1' register (12) ----*/
-#define BIT_TEMP_OUT                0x80
-#define BIT_GYRO_XOUT               0x40
-#define BIT_GYRO_YOUT               0x20
-#define BIT_GYRO_ZOUT               0x10
-#define BIT_ACCEL_XOUT              0x08
-#define BIT_ACCEL_YOUT              0x04
-#define BIT_ACCEL_ZOUT              0x02
-#define BIT_AUX_1OUT                0x01
-/*---- MPU 'FIFO_EN2' register (13) ----*/
-#define BIT_AUX_2OUT                0x02
-#define BIT_AUX_3OUT                0x01
-/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/
-#define BITS_EXT_SYNC_NONE          0x00
-#define BITS_EXT_SYNC_TEMP          0x20
-#define BITS_EXT_SYNC_GYROX         0x40
-#define BITS_EXT_SYNC_GYROY         0x60
-#define BITS_EXT_SYNC_GYROZ         0x80
-#define BITS_EXT_SYNC_ACCELX        0xA0
-#define BITS_EXT_SYNC_ACCELY        0xC0
-#define BITS_EXT_SYNC_ACCELZ        0xE0
-#define BITS_EXT_SYNC_MASK          0xE0
-#define BITS_FS_250DPS              0x00
-#define BITS_FS_500DPS              0x08
-#define BITS_FS_1000DPS             0x10
-#define BITS_FS_2000DPS             0x18
-#define BITS_FS_MASK                0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
-#define BITS_DLPF_CFG_188HZ         0x01
-#define BITS_DLPF_CFG_98HZ          0x02
-#define BITS_DLPF_CFG_42HZ          0x03
-#define BITS_DLPF_CFG_20HZ          0x04
-#define BITS_DLPF_CFG_10HZ          0x05
-#define BITS_DLPF_CFG_5HZ           0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
-#define BITS_DLPF_CFG_MASK          0x07
-/*---- MPU 'INT_CFG' register (17) ----*/
-#define BIT_ACTL                    0x80
-#define BIT_ACTL_LOW                0x80
-#define BIT_ACTL_HIGH               0x00
-#define BIT_OPEN                    0x40
-#define BIT_OPEN_DRAIN              0x40
-#define BIT_PUSH_PULL               0x00
-#define BIT_LATCH_INT_EN            0x20
-#define BIT_INT_PULSE_WIDTH_50US    0x00
-#define BIT_INT_ANYRD_2CLEAR        0x10
-#define BIT_INT_STAT_READ_2CLEAR    0x00
-#define BIT_MPU_RDY_EN              0x04
-#define BIT_DMP_INT_EN              0x02
-#define BIT_RAW_RDY_EN              0x01
-/*---- MPU 'INT_STATUS' register (1A) ----*/
-#define BIT_INT_STATUS_FIFO_OVERLOW 0x80
-#define BIT_MPU_RDY                 0x04
-#define BIT_DMP_INT                 0x02
-#define BIT_RAW_RDY                 0x01
-/*---- MPU 'BANK_SEL' register (37) ----*/
-#define BIT_PRFTCH_EN               0x20
-#define BIT_CFG_USER_BANK           0x10
-#define BITS_MEM_SEL                0x0f
-/*---- MPU 'USER_CTRL' register (3D) ----*/
-#define BIT_DMP_EN                  0x80
-#define BIT_FIFO_EN                 0x40
-#define BIT_AUX_IF_EN               0x20
-#define BIT_AUX_RD_LENG             0x10
-#define BIT_AUX_IF_RST              0x08
-#define BIT_DMP_RST                 0x04
-#define BIT_FIFO_RST                0x02
-#define BIT_GYRO_RST                0x01
-/*---- MPU 'PWR_MGM' register (3E) ----*/
-#define BIT_H_RESET                 0x80
-#define BIT_SLEEP                   0x40
-#define BIT_STBY_XG                 0x20
-#define BIT_STBY_YG                 0x10
-#define BIT_STBY_ZG                 0x08
-#define BITS_CLKSEL                 0x07
-
-/*---- MPU Silicon Revision ----*/
-#define MPU_SILICON_REV_A4           1	/* MPU A4 Device */
-#define MPU_SILICON_REV_B1           2	/* MPU B1 Device */
-#define MPU_SILICON_REV_B4           3	/* MPU B4 Device */
-#define MPU_SILICON_REV_B6           4	/* MPU B6 Device */
-
-/*---- MPU Memory ----*/
-#define MPU_MEM_BANK_SIZE           (256)
-#define FIFO_HW_SIZE                (512)
-
-enum MPU_MEMORY_BANKS {
-	MPU_MEM_RAM_BANK_0 = 0,
-	MPU_MEM_RAM_BANK_1,
-	MPU_MEM_RAM_BANK_2,
-	MPU_MEM_RAM_BANK_3,
-	MPU_MEM_NUM_RAM_BANKS,
-	MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS,
-	/* This one is always last */
-	MPU_MEM_NUM_BANKS
-};
-
-/*---- structure containing control variables used by MLDL ----*/
-/*---- MPU clock source settings ----*/
-/*---- MPU filter selections ----*/
-enum mpu_filter {
-	MPU_FILTER_256HZ_NOLPF2 = 0,
-	MPU_FILTER_188HZ,
-	MPU_FILTER_98HZ,
-	MPU_FILTER_42HZ,
-	MPU_FILTER_20HZ,
-	MPU_FILTER_10HZ,
-	MPU_FILTER_5HZ,
-	MPU_FILTER_2100HZ_NOLPF,
-	NUM_MPU_FILTER
-};
-
-enum mpu_fullscale {
-	MPU_FS_250DPS = 0,
-	MPU_FS_500DPS,
-	MPU_FS_1000DPS,
-	MPU_FS_2000DPS,
-	NUM_MPU_FS
-};
-
-enum mpu_clock_sel {
-	MPU_CLK_SEL_INTERNAL = 0,
-	MPU_CLK_SEL_PLLGYROX,
-	MPU_CLK_SEL_PLLGYROY,
-	MPU_CLK_SEL_PLLGYROZ,
-	MPU_CLK_SEL_PLLEXT32K,
-	MPU_CLK_SEL_PLLEXT19M,
-	MPU_CLK_SEL_RESERVED,
-	MPU_CLK_SEL_STOP,
-	NUM_CLK_SEL
-};
-
-enum mpu_ext_sync {
-	MPU_EXT_SYNC_NONE = 0,
-	MPU_EXT_SYNC_TEMP,
-	MPU_EXT_SYNC_GYROX,
-	MPU_EXT_SYNC_GYROY,
-	MPU_EXT_SYNC_GYROZ,
-	MPU_EXT_SYNC_ACCELX,
-	MPU_EXT_SYNC_ACCELY,
-	MPU_EXT_SYNC_ACCELZ,
-	NUM_MPU_EXT_SYNC
-};
-
-#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \
-	((ext_sync << 5) | (full_scale << 3) | lpf)
-
-#endif				/* __MPU3050_H_ */
diff --git a/mlsdk/platform/include/stdint_invensense.h b/mlsdk/platform/include/stdint_invensense.h
deleted file mode 100644
index d238813..0000000
--- a/mlsdk/platform/include/stdint_invensense.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 STDINT_INVENSENSE_H
-#define STDINT_INVENSENSE_H
-
-#ifndef WIN32
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include <stdint.h>
-#endif
-
-#else
-
-#include <windows.h>
-
-typedef char int8_t;
-typedef short int16_t;
-typedef long int32_t;
-
-typedef unsigned char uint8_t;
-typedef unsigned short uint16_t;
-typedef unsigned long uint32_t;
-
-typedef int int_fast8_t;
-typedef int int_fast16_t;
-typedef long int_fast32_t;
-
-typedef unsigned int uint_fast8_t;
-typedef unsigned int uint_fast16_t;
-typedef unsigned long uint_fast32_t;
-
-#endif
-
-#endif // STDINT_INVENSENSE_H
diff --git a/mlsdk/platform/linux/kernel/mpuirq.h b/mlsdk/platform/linux/kernel/mpuirq.h
deleted file mode 100644
index 352170b..0000000
--- a/mlsdk/platform/linux/kernel/mpuirq.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 __MPUIRQ__
-#define __MPUIRQ__
-
-#ifdef __KERNEL__
-#include <linux/time.h>
-#include <linux/ioctl.h>
-#include "mldl_cfg.h"
-#else
-#include <sys/ioctl.h>
-#include <sys/time.h>
-#endif
-
-#define MPUIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x40, unsigned long)
-#define MPUIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x41, unsigned long)
-#define MPUIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x42, struct timeval)
-#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
-
-#ifdef __KERNEL__
-void mpuirq_exit(void);
-int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg);
-#endif
-
-#endif
diff --git a/mlsdk/platform/linux/kernel/slaveirq.h b/mlsdk/platform/linux/kernel/slaveirq.h
deleted file mode 100644
index beb352b..0000000
--- a/mlsdk/platform/linux/kernel/slaveirq.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 __SLAVEIRQ__
-#define __SLAVEIRQ__
-
-#include <linux/mpu.h>
-#include "mpuirq.h"
-
-#define SLAVEIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x50, unsigned long)
-#define SLAVEIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x51, unsigned long)
-#define SLAVEIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x52, unsigned long)
-
-
-void slaveirq_exit(struct ext_slave_platform_data *pdata);
-int slaveirq_init(struct i2c_adapter *slave_adapter,
-		  struct ext_slave_platform_data *pdata, char *name);
-
-
-#endif
diff --git a/mlsdk/platform/linux/kernel/timerirq.h b/mlsdk/platform/linux/kernel/timerirq.h
deleted file mode 100644
index dc3eea2..0000000
--- a/mlsdk/platform/linux/kernel/timerirq.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- $License:
-   Copyright 2011 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 __TIMERIRQ__
-#define __TIMERIRQ__
-
-#include <linux/mpu.h>
-
-#define TIMERIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x60, unsigned long)
-#define TIMERIRQ_GET_INTERRUPT_CNT     _IOW(MPU_IOCTL, 0x61, unsigned long)
-#define TIMERIRQ_START                 _IOW(MPU_IOCTL, 0x62, unsigned long)
-#define TIMERIRQ_STOP                  _IO(MPU_IOCTL, 0x63)
-
-#endif
diff --git a/mlsdk/platform/linux/log_linux.c b/mlsdk/platform/linux/log_linux.c
deleted file mode 100644
index 8e75753..0000000
--- a/mlsdk/platform/linux/log_linux.c
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/******************************************************************************
- * $Id: log_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ 
- ******************************************************************************/
- 
-/**
- * @defgroup MPL_LOG
- * @brief Logging facility for the MPL
- *
- * @{
- *      @file     log.c
- *      @brief    Core logging facility functions.
- *
- *
-**/
-
-#include <stdio.h>
-#include <string.h>
-#include "log.h"
-#include "mltypes.h"
-
-#define LOG_BUFFER_SIZE (256)
-
-#ifdef WIN32
-#define snprintf _snprintf
-#define vsnprintf _vsnprintf
-#endif
-
-int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
-{
-    va_list ap;
-    int result;
-
-    va_start(ap, fmt);
-    result = _MLPrintVaLog(priority,tag,fmt,ap);
-    va_end(ap);
-
-    return result;
-}
-
-int _MLPrintVaLog(int priority, const char* tag, const char* fmt, va_list args)
-{
-    int result;
-    char buf[LOG_BUFFER_SIZE];
-    char new_fmt[LOG_BUFFER_SIZE];
-    char priority_char;
-
-    if (NULL == fmt) {
-        fmt = "";
-    }
-
-    switch (priority) {
-    case MPL_LOG_UNKNOWN:
-        priority_char = 'U';
-        break;
-    case MPL_LOG_VERBOSE:
-        priority_char = 'V';
-        break;
-    case MPL_LOG_DEBUG:
-        priority_char = 'D';
-        break;
-    case MPL_LOG_INFO:
-        priority_char = 'I';
-        break;
-    case MPL_LOG_WARN:
-        priority_char = 'W';
-        break;
-    case MPL_LOG_ERROR:
-        priority_char = 'E';
-        break;
-    case MPL_LOG_SILENT:
-        priority_char = 'S';
-        break;
-    case MPL_LOG_DEFAULT:
-    default:
-        priority_char = 'D';
-        break;
-    };
-
-    result = snprintf(new_fmt, sizeof(new_fmt), "%c/%s:%s", 
-                       priority_char, tag, fmt);
-    if (result <= 0) {
-        return INV_ERROR_LOG_MEMORY_ERROR;
-    }
-    result = vsnprintf(buf,sizeof(buf),new_fmt, args);
-    if (result <= 0) {
-        return INV_ERROR_LOG_OUTPUT_ERROR;
-    }
-    
-    result = _MLWriteLog(buf, strlen(buf));
-    return INV_SUCCESS;
-}
-
-/**
- * @}
-**/
-
-
diff --git a/mlsdk/platform/linux/log_printf_linux.c b/mlsdk/platform/linux/log_printf_linux.c
deleted file mode 100644
index 744a223..0000000
--- a/mlsdk/platform/linux/log_printf_linux.c
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-/*******************************************************************************
- *
- * $Id: log_printf_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ 
- *
- ******************************************************************************/
- 
-/**
- * @addtogroup MPL_LOG
- *
- * @{
- *      @file   log_printf.c
- *      @brief  printf replacement for _MLWriteLog.
- */
-
-#include <stdio.h>
-#include "log.h"
-
-int _MLWriteLog (const char * buf, int buflen)
-{
-    return fputs(buf, stdout);
-}
-
-/**
- * @}
- */
-
diff --git a/mlsdk/platform/linux/mlsl_linux_mpu.c b/mlsdk/platform/linux/mlsl_linux_mpu.c
deleted file mode 100644
index 29930b3..0000000
--- a/mlsdk/platform/linux/mlsl_linux_mpu.c
+++ /dev/null
@@ -1,497 +0,0 @@
-/*
- $License:
-   Copyright 2011 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.
-  $
- */
-
-/******************************************************************************
- * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
- *****************************************************************************/
-
-/**
- *  @defgroup MLSL (Motion Library - Serial Layer)
- *  @brief  The Motion Library System Layer provides the Motion Library the
- *          interface to the system functions.
- *
- *  @{
- *      @file   mlsl_linux_mpu.c
- *      @brief  The Motion Library System Layer.
- *
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-#include <stdio.h>
-#include <sys/ioctl.h>
-#include <stdlib.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <unistd.h>
-#include <linux/fs.h>
-#include <linux/i2c.h>
-#include <string.h>
-#include <signal.h>
-#include <time.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-#    include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-#    include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-#  include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlmath.h"
-#include "mlinclude.h"
-
-#define MLCAL_ID      (0x0A0B0C0DL)
-#define MLCAL_FILE    "/data/cal.bin"
-#define MLCFG_ID      (0x01020304L)
-#define MLCFG_FILE    "/data/cfg.bin"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mlsl"
-
-#ifndef I2CDEV
-#define I2CDEV "/dev/mpu"
-#endif
-
-#define SERIAL_FULL_DEBUG (0)
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* ----------------------- */
-/* -  Function Pointers. - */
-/* ----------------------- */
-
-/* --------------------------- */
-/* - Global and Static vars. - */
-/* --------------------------- */
-
-/* ---------------- */
-/* - Definitions. - */
-/* ---------------- */
-
-inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len)
-{
-    FILE *fp;
-    int bytesRead;
-
-    fp = fopen(MLCFG_FILE, "rb");
-    if (fp == NULL) {
-        MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE);
-        return INV_ERROR_FILE_OPEN;
-    }
-    bytesRead = fread(cfg, 1, len, fp);
-    if (bytesRead != len) {
-        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
-                 bytesRead, len);
-        return INV_ERROR_FILE_READ;
-    }
-    fclose(fp);
-
-    if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3])
-        != MLCFG_ID) {
-        return INV_ERROR_ASSERTION_FAILURE;
-    }
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len)
-{
-    FILE *fp;
-    int bytesWritten;
-    unsigned char cfgId[4];
-
-    fp = fopen(MLCFG_FILE,"wb");
-    if (fp == NULL) {
-        MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE);
-        return INV_ERROR_FILE_OPEN;
-    }
-
-    cfgId[0] = (unsigned char)(MLCFG_ID >> 24);
-    cfgId[1] = (unsigned char)(MLCFG_ID >> 16);
-    cfgId[2] = (unsigned char)(MLCFG_ID >> 8);
-    cfgId[3] = (unsigned char)(MLCFG_ID);
-    bytesWritten = fwrite(cfgId, 1, 4, fp);
-    if (bytesWritten != 4) {
-        MPL_LOGE("CFG ID could not be written on file\n");
-        return INV_ERROR_FILE_WRITE;
-    }
-
-    bytesWritten = fwrite(cfg, 1, len, fp);
-    if (bytesWritten != len) {
-        MPL_LOGE("bytes write (%d) don't match requested length (%d)\n",
-                 bytesWritten, len);
-        return INV_ERROR_FILE_WRITE;
-    }
-
-    fclose(fp);
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len)
-{
-    FILE *fp;
-    int bytesRead;
-    inv_error_t result = INV_SUCCESS;
-
-    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);
-        result = INV_ERROR_FILE_READ;
-        goto read_cal_end;
-    }
-
-    /* MLCAL_ID not used
-    if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3])
-        != MLCAL_ID) {
-        result = INV_ERROR_ASSERTION_FAILURE;
-        goto read_cal_end;
-    }
-    */
-read_cal_end:
-    fclose(fp);
-    return result;
-}
-
-inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len)
-{
-    FILE *fp;
-    int bytesWritten;
-    inv_error_t result = INV_SUCCESS;
-
-    fp = fopen(MLCAL_FILE,"wb");
-    if (fp == NULL) {
-        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
-        return INV_ERROR_FILE_OPEN;
-    }
-    bytesWritten = fwrite(cal, 1, len, fp);
-    if (bytesWritten != len) {
-        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
-                 bytesWritten, len);
-        result = INV_ERROR_FILE_WRITE;
-    }
-    fclose(fp);
-    return result;
-}
-
-inv_error_t inv_serial_get_cal_length(unsigned int *len)
-{
-    FILE *calFile;
-    *len = 0;
-
-    calFile = fopen(MLCAL_FILE, "rb");
-    if (calFile == NULL) {
-        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
-        return INV_ERROR_FILE_OPEN;
-    }
-
-    *len += (unsigned char)fgetc(calFile) << 24;
-    *len += (unsigned char)fgetc(calFile) << 16;
-    *len += (unsigned char)fgetc(calFile) << 8;
-    *len += (unsigned char)fgetc(calFile);
-
-    fclose(calFile);
-
-    if (*len <= 0)
-        return INV_ERROR_FILE_READ;
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_open(char const *port, void **sl_handle)
-{
-    INVENSENSE_FUNC_START;
-
-    if (NULL == port) {
-        port = I2CDEV;
-    }
-    *sl_handle = (void*) open(port, O_RDWR);
-    if(sl_handle < 0) {
-        /* ERROR HANDLING; you can check errno to see what went wrong */
-        MPL_LOGE("inv_serial_open\n");
-        MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port);
-        return INV_ERROR_SERIAL_OPEN_ERROR;
-    } else {
-        MPL_LOGI("inv_serial_open: %s\n", port);
-    }
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_close(void *sl_handle)
-{
-    INVENSENSE_FUNC_START;
-
-    close((int)sl_handle);
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_reset(void *sl_handle)
-{
-    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-}
-
-inv_error_t inv_serial_single_write(void *sl_handle,
-                               unsigned char slaveAddr,
-                               unsigned char registerAddr,
-                               unsigned char data)
-{
-    unsigned char buf[2];
-    buf[0] = registerAddr;
-    buf[1] = data;
-    return inv_serial_write(sl_handle, slaveAddr, 2, buf);
-}
-
-inv_error_t inv_serial_write(void *sl_handle,
-                         unsigned char slaveAddr,
-                         unsigned short length,
-                         unsigned char const *data)
-{
-    INVENSENSE_FUNC_START;
-    struct mpu_read_write msg;
-    inv_error_t result;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    msg.address = 0; /* not used */
-    msg.length  = length;
-    msg.data    = (unsigned char*)data;
-
-    if ((result = ioctl((int)sl_handle, MPU_WRITE, &msg))) {
-        MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n",
-                 data[0], length, result);
-       return result;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C Write Success %02x %02x: %s \n",
-                 data[0], length, data_buff);
-    }
-
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read(void *sl_handle,
-                        unsigned char  slaveAddr,
-                        unsigned char  registerAddr,
-                        unsigned short length,
-                        unsigned char  *data)
-{
-    INVENSENSE_FUNC_START;
-    int result = INV_SUCCESS;
-    struct mpu_read_write msg;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    msg.address = registerAddr;
-    msg.length  = length;
-    msg.data    = data;
-
-    result = ioctl((int)sl_handle, MPU_READ, &msg);
-
-    if (result != INV_SUCCESS) {
-        MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n",
-                 result, registerAddr, length);
-        result = INV_ERROR_SERIAL_READ;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C Read  Success %02x %02x: %s \n",
-                  registerAddr, length, data_buff);
-    }
-
-    return (inv_error_t) result;
-}
-
-inv_error_t inv_serial_write_mem(void *sl_handle,
-                            unsigned char mpu_addr,
-                            unsigned short memAddr,
-                            unsigned short length,
-                            const unsigned char *data)
-{
-    INVENSENSE_FUNC_START;
-    struct mpu_read_write msg;
-    int result;
-
-    msg.address = memAddr;
-    msg.length  = length;
-    msg.data    = (unsigned char *)data;
-
-    result = ioctl((int)sl_handle, MPU_WRITE_MEM, &msg);
-    if (result) {
-        LOG_RESULT_LOCATION(result);
-        return result;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n",
-                 memAddr, length, data_buff);
-    }
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read_mem(void *sl_handle,
-                           unsigned char  mpu_addr,
-                           unsigned short memAddr,
-                           unsigned short length,
-                           unsigned char  *data)
-{
-    INVENSENSE_FUNC_START;
-    struct mpu_read_write msg;
-    int result;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    msg.address = memAddr;
-    msg.length  = length;
-    msg.data    = data;
-
-    result = ioctl((int)sl_handle, MPU_READ_MEM, &msg);
-    if (result != INV_SUCCESS) {
-        MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n",
-                 result, memAddr, length);
-        return INV_ERROR_SERIAL_READ;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n",
-                 memAddr, length, data_buff);
-    }
-    return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_write_fifo(void *sl_handle,
-                             unsigned char mpu_addr,
-                             unsigned short length,
-                             const unsigned char *data)
-{
-    INVENSENSE_FUNC_START;
-    struct mpu_read_write msg;
-    int result;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    msg.address = 0; /* Not used */
-    msg.length  = length;
-    msg.data    = (unsigned char *)data;
-
-    result = ioctl((int)sl_handle, MPU_WRITE_FIFO, &msg);
-    if (result != INV_SUCCESS) {
-        MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n",
-                  MPUREG_FIFO_R_W, length);
-        return INV_ERROR_SERIAL_WRITE;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C Write Success %02x %02x: %s\n",
-                 MPUREG_FIFO_R_W, length, data_buff);
-    }
-    return (inv_error_t) result;
-}
-
-inv_error_t inv_serial_read_fifo(void *sl_handle,
-                            unsigned char  mpu_addr,
-                            unsigned short length,
-                            unsigned char  *data)
-{
-    INVENSENSE_FUNC_START;
-    struct mpu_read_write msg;
-    int result;
-
-    if (NULL == data) {
-        return INV_ERROR_INVALID_PARAMETER;
-    }
-
-    msg.address = MPUREG_FIFO_R_W; /* Not used */
-    msg.length  = length;
-    msg.data    = data;
-
-    result = ioctl((int)sl_handle, MPU_READ_FIFO, &msg);
-    if (result != INV_SUCCESS) {
-        MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n",
-                 result, MPUREG_FIFO_R_W, length);
-        return INV_ERROR_SERIAL_READ;
-    } else if (SERIAL_FULL_DEBUG) {
-        char data_buff[4096] = "";
-        char strchar[3];
-        int ii;
-        for (ii = 0; ii < length; ii++) {
-            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
-            strncat(data_buff, strchar, sizeof(data_buff));
-        }
-        MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
-                 MPUREG_FIFO_R_W, length, data_buff);
-    }
-    return INV_SUCCESS;
-}
-
-/**
- *  @}
- */
-
-