sensors: Merge invensense HAL into main tuna HAL.

Since these are both in our device tree now, there's no need to
have them be separated. This saves about 14KB of space as well.

Change-Id: Ibfcd7da4b30bb261586ecd9373e6fd4a343e0e06

73 files changed, 24058 insertions, 8 deletions

diff --git a/libsensors/Android.mk b/libsensors/Android.mk
index 98d03b0..2d2d274 100644
--- a/libsensors/Android.mk
+++ b/libsensors/Android.mk
@@ -11,10 +11,10 @@
 # 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.
-
-
 LOCAL_PATH := $(call my-dir)
 
+include $(call all-named-subdir-makefiles,mlsdk)
+
 # HAL module implemenation stored in
 # hw/<SENSORS_HARDWARE_MODULE_ID>.<ro.product.board>.so
 include $(CLEAR_VARS)
@@ -25,10 +25,19 @@ LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
 
 LOCAL_MODULE_TAGS := optional
 
-LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
-LOCAL_C_INCLUDES := $(LOCAL_PATH)/../invensense/libinvensense_hal/
+LOCAL_C_INCLUDES := \
+	$(LOCAL_PATH)/mlsdk/platform/include \
+	$(LOCAL_PATH)/mlsdk/platform/include/linux \
+	$(LOCAL_PATH)/mlsdk/platform/linux \
+	$(LOCAL_PATH)/mlsdk/mllite \
+	$(LOCAL_PATH)/mlsdk/mldmp \
+	$(LOCAL_PATH)/mlsdk/external/aichi \
+	$(LOCAL_PATH)/mlsdk/external/akmd
+
 LOCAL_SRC_FILES := \
 	sensors.cpp \
+	SensorBase.cpp \
+	MPLSensor.cpp \
 	InputEventReader.cpp \
 	LightSensor.cpp \
 	ProximitySensor.cpp \
@@ -36,7 +45,10 @@ LOCAL_SRC_FILES := \
 	SamsungSensorBase.cpp \
 	TemperatureSensor.cpp
 
-LOCAL_SHARED_LIBRARIES := libinvensense_hal.$(TARGET_BOOTLOADER_BOARD_NAME) liblog libcutils libutils libdl
+LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+LOCAL_CPPFLAGS := -DLINUX=1
+LOCAL_LDFLAGS := -rdynamic
 LOCAL_CLANG := true
 LOCAL_CFLAGS += -Wall -Werror
 
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp
new file mode 100644
index 0000000..ca08ad5
--- /dev/null
+++ b/libsensors/MPLSensor.cpp
@@ -0,0 +1,1128 @@
+/*
+ * 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
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <float.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <dlfcn.h>
+#include <pthread.h>
+
+#include <cutils/log.h>
+#include <utils/KeyedVector.h>
+
+#include "MPLSensor.h"
+
+#include "math.h"
+#include "ml.h"
+#include "mlFIFO.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include "ml_mputest.h"
+#include "ml_stored_data.h"
+#include "mldl_cfg.h"
+#include "mldl.h"
+
+#include "mpu.h"
+#include "accel.h"
+#include "compass.h"
+#include "kernel/timerirq.h"
+#include "kernel/mpuirq.h"
+#include "kernel/slaveirq.h"
+
+extern "C" {
+#include "mlsupervisor.h"
+}
+
+#include "mlcontrol.h"
+#include "sensor_params.h"
+
+#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 CALL_MEMBER_FN(pobject,ptrToMember)  ((pobject)->*(ptrToMember))
+
+/******************************************/
+
+/* 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, 0, 0,
+     SENSOR_STRING_TYPE_GYROSCOPE, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Accelerometer", "Invensense", 1, SENSORS_ACCELERATION_HANDLE,
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_ACCELEROMETER, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Magnetic Field", "Invensense", 1, SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_MAGNETIC_FIELD, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Orientation", "Invensense", 1, SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_ORIENTATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Rotation Vector", "Invensense", 1, SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_ORIENTATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Linear Acceleration", "Invensense", 1, SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_LINEAR_ACCELERATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+    {"MPL Gravity", "Invensense", 1, SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0,
+     SENSOR_STRING_TYPE_GRAVITY, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}},
+};
+
+static unsigned long long irq_timestamp = 0;
+/* ***************************************************************************
+ * MPL interface misc.
+ */
+//static pointer to the object that will handle callbacks
+static 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();
+    }
+}
+
+} //end of extern C
+
+void setCallbackObject(MPLSensor* gbpt)
+{
+    gMPLSensor = gbpt;
+}
+
+
+/*****************************************************************************
+ * sensor class implementation
+ */
+
+#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),
+            mNewData(0),
+            mDmpStarted(false),
+            mMasterSensorMask(INV_ALL_SENSORS),
+            mLocalSensorMask(ALL_MPL_SENSORS_NP),
+            mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
+            mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
+            mUseTimerirq(false),
+            mEnabled(0), mPendingMask(0)
+{
+    FUNC_LOG;
+    int mpu_int_fd;
+    char *port = NULL;
+
+    ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
+
+    pthread_mutex_init(&mMplMutex, NULL);
+
+    mForceSleep = false;
+
+    /* 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;
+
+    for (size_t i = 0; i < ARRAY_SIZE(mPollFds); i++) {
+        mPollFds[i].fd = -1;
+        mPollFds[i].events = 0;
+    }
+
+    pthread_mutex_lock(&mMplMutex);
+
+    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);
+        mIrqFds.add(MPUIRQ_FD, mpu_int_fd);
+        mPollFds[MPUIRQ_FD].fd = mpu_int_fd;
+        mPollFds[MPUIRQ_FD].events = POLLIN;
+    }
+
+    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);
+        mIrqFds.add(ACCELIRQ_FD, accel_fd);
+        mPollFds[ACCELIRQ_FD].fd = accel_fd;
+        mPollFds[ACCELIRQ_FD].events = POLLIN;
+    }
+
+    timer_fd = open("/dev/timerirq", O_RDWR);
+    if (timer_fd == -1) {
+        ALOGE("could not open the timer irq device node");
+    } else {
+        fcntl(timer_fd, F_SETFL, O_NONBLOCK);
+        mIrqFds.add(TIMERIRQ_FD, timer_fd);
+        mPollFds[TIMERIRQ_FD].fd = timer_fd;
+        mPollFds[TIMERIRQ_FD].events = POLLIN;
+    }
+
+    data_fd = mpu_int_fd;
+
+    if ((accel_fd == -1) && (timer_fd != -1)) {
+        //no accel irq and timer available
+        mUseTimerIrqAccel = true;
+    }
+
+    memset(mPendingEvents, 0, sizeof(mPendingEvents));
+
+    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
+    mPendingEvents[RotationVector].sensor = ID_RV;
+    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
+
+    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
+    mPendingEvents[LinearAccel].sensor = ID_LA;
+    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+
+    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+    mPendingEvents[Gravity].sensor = ID_GR;
+    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+
+    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+    mPendingEvents[Gyro].sensor = ID_GY;
+    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+
+    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[Orientation].version = sizeof(sensors_event_t);
+    mPendingEvents[Orientation].sensor = ID_O;
+    mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
+    mPendingEvents[Orientation].orientation.status
+            = SENSOR_STATUS_ACCURACY_HIGH;
+
+    mHandlers[RotationVector] = &MPLSensor::rvHandler;
+    mHandlers[LinearAccel] = &MPLSensor::laHandler;
+    mHandlers[Gravity] = &MPLSensor::gravHandler;
+    mHandlers[Gyro] = &MPLSensor::gyroHandler;
+    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
+    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");
+    }
+
+    //initialize library parameters
+    initMPL();
+
+    //setup the FIFO contents
+    setupFIFO();
+
+
+    pthread_mutex_unlock(&mMplMutex);
+}
+
+MPLSensor::~MPLSensor()
+{
+    FUNC_LOG;
+    pthread_mutex_lock(&mMplMutex);
+    if (inv_dmp_stop() != INV_SUCCESS) {
+        ALOGW("Error: could not stop the DMP correctly.\n");
+    }
+
+    if (inv_dmp_close() != INV_SUCCESS) {
+        ALOGW("Error: could not close the DMP");
+    }
+
+    if (inv_serial_stop() != INV_SUCCESS) {
+        ALOGW("Error : could not close the serial port");
+    }
+    pthread_mutex_unlock(&mMplMutex);
+    pthread_mutex_destroy(&mMplMutex);
+}
+
+/* clear any data from our various filehandles */
+void MPLSensor::clearIrqData(bool* irq_set)
+{
+    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;
+        if (mPollFds[i].revents & POLLIN) {
+            nread = read(cur_fd, &irqdata, sizeof(irqdata));
+            if (nread > 0) {
+                irq_set[i] = true;
+                irq_timestamp = irqdata.irqtime;
+            }
+        }
+        mPollFds[i].revents = 0;
+    }
+}
+
+/* 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)
+{
+    FUNC_LOG;
+    bool irq_set[5] = { false, false, false, false, false };
+
+    do {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            mLocalSensorMask = ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_ENABLED) {
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+        } else {
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+
+        if (A_ENABLED) {
+            mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
+        } else {
+            mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
+        }
+
+        if (M_ENABLED) {
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+        } else {
+            mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
+        }
+    } while (0);
+
+    //record the new sensor state
+    inv_error_t rv;
+
+    unsigned long sen_mask = mLocalSensorMask & mMasterSensorMask;
+
+    bool changing_sensors = ((inv_get_dl_config()->requested_sensors
+            != sen_mask) && (sen_mask != 0));
+    bool restart = (!mDmpStarted) && (sen_mask != 0);
+
+    if (changing_sensors || restart) {
+
+        ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);
+
+        if (mDmpStarted) {
+            inv_dmp_stop();
+            clearIrqData(irq_set);
+            mDmpStarted = false;
+        }
+
+        if (sen_mask != inv_get_dl_config()->requested_sensors) {
+            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);
+        }
+
+        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;
+        }
+
+        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;
+            }
+            rv = inv_dmp_start();
+            ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
+            mDmpStarted = true;
+        }
+    }
+
+    //check if we should stop the DMP
+    if (mDmpStarted && (sen_mask == 0)) {
+        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);
+        }
+        clearIrqData(irq_set);
+
+        mDmpStarted = false;
+        mCurFifoRate = -1;
+    }
+}
+
+/**
+ * container function for all the calls we make once to set up the MPL.
+ */
+void MPLSensor::initMPL()
+{
+    FUNC_LOG;
+    inv_error_t result;
+    unsigned short bias_update_mask = 0xFFFF;
+
+    if (inv_dmp_open() != INV_SUCCESS) {
+        ALOGE("Fatal Error : could not open DMP correctly.\n");
+    }
+
+    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");
+    }
+
+    //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;
+        }
+    } else {
+        const char* error = dlerror();
+        ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
+    }
+
+    if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
+        ALOGE("Error : Bias update function could not be set.\n");
+    }
+
+    if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
+        ALOGE("Error : could not set motion interrupt");
+    }
+
+    if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
+        ALOGE("Error : could not set fifo interrupt");
+    }
+
+    result = inv_set_fifo_rate(6);
+    if (result != INV_SUCCESS) {
+        ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
+    }
+
+    setupCallbacks();
+}
+
+/** setup the fifo contents.
+ */
+void MPLSensor::setupFIFO()
+{
+    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);
+    }
+
+    result = inv_send_quaternion(INV_32_BIT);
+    if (result != INV_SUCCESS) {
+        ALOGE("Fatal error: inv_send_quaternion returned %d\n", result);
+    }
+
+    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);
+    }
+
+    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);
+    }
+
+    result = inv_send_gravity(INV_ALL, INV_32_BIT);
+    if (result != INV_SUCCESS) {
+        ALOGE("Fatal error: inv_send_gravity returned %d\n", result);
+    }
+
+    result = inv_send_gyro(INV_ALL, INV_32_BIT);
+    if (result != INV_SUCCESS) {
+        ALOGE("Fatal error: inv_send_gyro returned %d\n", result);
+    }
+}
+
+/**
+ *  set up the callbacks that we use in all cases (outside of gestures, etc)
+ */
+void MPLSensor::setupCallbacks()
+{
+    FUNC_LOG;
+    if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) {
+        ALOGE("Error : Motion callback could not be set.\n");
+
+    }
+
+    if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
+        ALOGE("Error : Processed data callback could not be set.");
+
+    }
+}
+
+/**
+ * handle the motion/no motion output from the MPL.
+ */
+void MPLSensor::cbOnMotion(uint16_t val)
+{
+    FUNC_LOG;
+    //after the first no motion, the gyro should be calibrated well
+    if (val == 2) {
+        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;
+        }
+    }
+
+    return;
+}
+
+
+void MPLSensor::cbProcData()
+{
+    mNewData = 1;
+}
+
+//these handlers transform mpl data into one of the Android sensor types
+//  scaling and coordinate transforms should be done in the handlers
+
+void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask,
+                             int index)
+{
+    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;
+    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;
+    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;
+}
+
+void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask,
+                                int index)
+{
+    VFUNC_LOG;
+    inv_error_t res;
+
+    res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
+
+    if (res != INV_SUCCESS) {
+        ALOGW(
+             "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d",
+             res);
+    }
+
+    s->magnetic.status = estimateCompassAccuracy();
+
+    if (res == INV_SUCCESS)
+        *pending_mask |= (1 << index);
+}
+
+void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask,
+                           int index)
+{
+    VFUNC_LOG;
+    float quat[4];
+    float norm = 0;
+    inv_error_t r;
+
+    r = inv_get_float_array(INV_QUATERNION, quat);
+
+    if (r != INV_SUCCESS) {
+        *pending_mask &= ~(1 << index);
+        return;
+    } else {
+        *pending_mask |= (1 << index);
+    }
+
+    norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]
+            + FLT_EPSILON;
+
+    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;
+    }
+
+    if (quat[0] < 0.0) {
+        quat[1] = -quat[1];
+        quat[2] = -quat[2];
+        quat[3] = -quat[3];
+    }
+
+    s->gyro.v[0] = quat[1];
+    s->gyro.v[1] = quat[2];
+    s->gyro.v[2] = quat[3];
+}
+
+void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
+                           int index)
+{
+    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;
+    if (res == INV_SUCCESS)
+        *pending_mask |= (1 << index);
+}
+
+void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask,
+                             int index)
+{
+    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;
+    if (res == INV_SUCCESS)
+        *pending_mask |= (1 << index);
+}
+
+void MPLSensor::calcOrientationSensor(float *R, float *values)
+{
+    float tmp;
+
+    //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]);
+    }
+
+    values[2] *= 57.295779513082320876798154814105f;
+    if (values[2] > 90.0f) {
+        values[2] = 180.0f - values[2];
+    }
+    if (values[2] < -90.0f) {
+        values[2] = -180.0f - values[2];
+    }
+}
+
+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
+{
+    VFUNC_LOG;
+    inv_error_t res;
+    float rot_mat[9];
+
+    res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
+
+    calcOrientationSensor(rot_mat, s->orientation.v);
+
+    s->orientation.status = estimateCompassAccuracy();
+
+    if (res == INV_SUCCESS)
+        *pending_mask |= (1 << index);
+    else
+        ALOGW("orienHandler: data not valid (%d)", (int) res);
+}
+
+int MPLSensor::enable(int32_t handle, int en)
+{
+    FUNC_LOG;
+
+    int what = -1;
+
+    switch (handle) {
+    case ID_A:
+        what = Accelerometer;
+        break;
+    case ID_M:
+        what = MagneticField;
+        break;
+    case ID_O:
+        what = Orientation;
+        break;
+    case ID_GY:
+        what = Gyro;
+        break;
+    case ID_GR:
+        what = Gravity;
+        break;
+    case ID_RV:
+        what = RotationVector;
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        break;
+    default: //this takes care of all the gestures
+        what = handle;
+        break;
+    }
+
+    if (uint32_t(what) >= numSensors)
+        return -EINVAL;
+
+    int newState = en ? 1 : 0;
+    int err = 0;
+
+    pthread_mutex_lock(&mMplMutex);
+    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
+        short flags = newState;
+        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);
+    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));
+    int what = -1;
+    switch (handle) {
+    case ID_A:
+        what = Accelerometer;
+        break;
+    case ID_M:
+        what = MagneticField;
+        break;
+    case ID_O:
+        what = Orientation;
+        break;
+    case ID_GY:
+        what = Gyro;
+        break;
+    case ID_GR:
+        what = Gravity;
+        break;
+    case ID_RV:
+        what = RotationVector;
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        break;
+    default:
+        what = handle;
+        break;
+    }
+
+    if (uint32_t(what) >= numSensors)
+        return -EINVAL;
+
+    if (ns < 0)
+        return -EINVAL;
+
+    pthread_mutex_lock(&mMplMutex);
+    mDelays[what] = ns;
+    pthread_mutex_unlock(&mMplMutex);
+    return update_delay();
+}
+
+int MPLSensor::update_delay()
+{
+    FUNC_LOG;
+    int rv = 0;
+    bool irq_set[5];
+
+    pthread_mutex_lock(&mMplMutex);
+
+    if (mEnabled) {
+        uint64_t wanted = -1LLU;
+        for (int i = 0; i < numSensors; i++) {
+            if (mEnabled & (1 << i)) {
+                uint64_t ns = mDelays[i];
+                wanted = wanted < ns ? wanted : ns;
+            }
+        }
+
+        //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
+        if (wanted < 10000000LLU) {
+            wanted = 10000000LLU;
+        }
+
+        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;
+
+        if (rate != mCurFifoRate) {
+            inv_error_t res; // = inv_dmp_stop();
+            res = inv_set_fifo_rate(rate);
+            ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
+
+            mCurFifoRate = rate;
+            rv = (res == INV_SUCCESS);
+        }
+
+        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));
+                } 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());
+                }
+            }
+        }
+
+    }
+    pthread_mutex_unlock(&mMplMutex);
+    return rv;
+}
+
+int MPLSensor::readEvents(sensors_event_t* data, int count)
+{
+    //VFUNC_LOG;
+    bool irq_set[5] = { false, false, false, false, false };
+    inv_error_t rv;
+    if (count < 1)
+        return -EINVAL;
+    int numEventReceived = 0;
+
+    clearIrqData(irq_set);
+
+    pthread_mutex_lock(&mMplMutex);
+    if (mDmpStarted) {
+        rv = inv_update_data();
+        ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
+    }
+
+    else {
+        //probably just one extra read after shutting down
+        ALOGV_IF(EXTRA_VERBOSE,
+                "MPLSensor::readEvents called, but there's nothing to do.");
+    }
+
+    pthread_mutex_unlock(&mMplMutex);
+
+    if (!mNewData) {
+        ALOGV_IF(EXTRA_VERBOSE, "no new data");
+        return 0;
+    }
+    mNewData = 0;
+
+    /* google timestamp */
+    pthread_mutex_lock(&mMplMutex);
+    for (int i = 0; i < numSensors; i++) {
+        if (mEnabled & (1 << i)) {
+            CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i,
+                                              &mPendingMask, i);
+            mPendingEvents[i].timestamp = irq_timestamp;
+        }
+    }
+
+    for (int j = 0; count && mPendingMask && j < numSensors; j++) {
+        if (mPendingMask & (1 << j)) {
+            mPendingMask &= ~(1 << j);
+            if (mEnabled & (1 << j)) {
+                *data++ = mPendingEvents[j];
+                count--;
+                numEventReceived++;
+            }
+        }
+    }
+
+    pthread_mutex_unlock(&mMplMutex);
+    return numEventReceived;
+}
+
+int MPLSensor::getFd() const
+{
+    return data_fd;
+}
+
+int MPLSensor::getAccelFd() const
+{
+    return accel_fd;
+}
+
+int MPLSensor::getTimerFd() const
+{
+    return timer_fd;
+}
+
+int MPLSensor::getPowerFd() const
+{
+    return (uintptr_t) inv_get_serial_handle();
+}
+
+void MPLSensor::handlePowerEvent()
+{
+    VFUNC_LOG;
+    mpuirq_data irqd;
+
+    int fd = (uintptr_t) inv_get_serial_handle();
+    read(fd, &irqd, sizeof(irqd));
+
+    if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) {
+        //going to sleep
+        sleepEvent();
+    } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) {
+        //waking up
+        wakeEvent();
+    }
+
+    ioctl(fd, MPU_PM_EVENT_HANDLED, 0);
+}
+
+void MPLSensor::sleepEvent()
+{
+    VFUNC_LOG;
+    pthread_mutex_lock(&mMplMutex);
+    if (mEnabled != 0) {
+        mForceSleep = true;
+        mOldEnabledMask = mEnabled;
+        setPowerStates(0);
+    }
+    pthread_mutex_unlock(&mMplMutex);
+}
+
+void MPLSensor::wakeEvent()
+{
+    VFUNC_LOG;
+    pthread_mutex_lock(&mMplMutex);
+    if (mForceSleep) {
+        setPowerStates((mOldEnabledMask | mEnabled));
+    }
+    mForceSleep = false;
+    pthread_mutex_unlock(&mMplMutex);
+}
+
+/** fill in the sensor list based on which sensors are configured.
+ *  return the number of configured sensors.
+ *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
+ *  parameter len gives the length of the buffer pointed to by list
+ */
+
+int MPLSensor::populateSensorList(struct sensor_t *list, size_t len)
+{
+    int numsensors;
+
+    if(len < 7*sizeof(sensor_t)) {
+        ALOGE("sensor list too small, not populating.");
+        return 0;
+    }
+
+    /* fill in the base values */
+    memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
+
+    /* first add gyro, accel and compass to the list */
+
+    /* fill in accel values                          */
+    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+    list[Accelerometer].power = ACCEL_BMA250_POWER;
+
+    /* fill in compass values                        */
+    list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
+    list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
+    list[MagneticField].power = COMPASS_YAS530_POWER;
+
+    /* fill in gyro values                           */
+    list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+    list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+    list[Gyro].power = GYRO_MPU3050_POWER;
+
+    if(mNineAxisEnabled)
+    {
+        numsensors = 7;
+        /* all sensors will be added to the list     */
+        /* fill in orientation values	             */
+        fillOrientation(list);
+
+        /* fill in rotation vector values	     */
+        fillRV(list);
+
+        /* fill in gravity values			     */
+        fillGravity(list);
+
+        /* 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));
+    }
+
+    return numsensors;
+}
+
+/* fillRV depends on values of accel and compass in the list	*/
+void MPLSensor::fillRV(struct sensor_t *list)
+{
+    /* compute power on the fly */
+    list[RotationVector].power = list[Gyro].power + list[Accelerometer].power
+            + list[MagneticField].power;
+    list[RotationVector].resolution = .00001;
+    list[RotationVector].maxRange = 1.0;
+    return;
+}
+
+void MPLSensor::fillOrientation(struct sensor_t *list)
+{
+    list[Orientation].power = list[Gyro].power + list[Accelerometer].power
+            + list[MagneticField].power;
+    list[Orientation].resolution = .00001;
+    list[Orientation].maxRange = 360.0;
+    return;
+}
+
+void MPLSensor::fillGravity( struct sensor_t *list)
+{
+    list[Gravity].power = list[Gyro].power + list[Accelerometer].power
+            + list[MagneticField].power;
+    list[Gravity].resolution = .00001;
+    list[Gravity].maxRange = 9.81;
+    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;
+    return;
+}
diff --git a/libsensors/MPLSensor.h b/libsensors/MPLSensor.h
new file mode 100644
index 0000000..ea72c72
--- /dev/null
+++ b/libsensors/MPLSensor.h
@@ -0,0 +1,133 @@
+/*
+ * 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 void handlePowerEvent();
+    virtual void sleepEvent();
+    virtual void wakeEvent();
+    int populateSensorList(struct sensor_t *list, size_t 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 mNewData; //flag indicating that the MPL calculated new output values
+    int mDmpStarted;
+    long mMasterSensorMask;
+    long mLocalSensorMask;
+    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];
+    pthread_mutex_t mMplMutex;
+
+    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 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
diff --git a/libsensors/SensorBase.cpp b/libsensors/SensorBase.cpp
new file mode 100644
index 0000000..6d6ab25
--- /dev/null
+++ b/libsensors/SensorBase.cpp
@@ -0,0 +1,122 @@
+/*
+ * 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 <string.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 __unused, int64_t ns __unused) {
+    return 0;
+}
+
+bool SensorBase::hasPendingEvents() const {
+    return false;
+}
+
+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;
+}
diff --git a/libsensors/SensorBase.h b/libsensors/SensorBase.h
new file mode 100644
index 0000000..eb50550
--- /dev/null
+++ b/libsensors/SensorBase.h
@@ -0,0 +1,59 @@
+/*
+ * 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_SENSOR_BASE_H
+#define ANDROID_SENSOR_BASE_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+
+/*****************************************************************************/
+
+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;
+
+    int openInput(const char* inputName);
+
+    int open_device();
+    int close_device();
+
+public:
+            SensorBase(
+                    const char* dev_name,
+                    const char* data_name);
+
+    virtual ~SensorBase();
+
+    virtual int readEvents(sensors_event_t* data, int count) = 0;
+    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;
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_SENSOR_BASE_H
diff --git a/libsensors/mlsdk/Android.mk b/libsensors/mlsdk/Android.mk
new file mode 100644
index 0000000..d8c0724
--- /dev/null
+++ b/libsensors/mlsdk/Android.mk
@@ -0,0 +1,76 @@
+MLSDK_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_MODULE := libmlplatform
+
+LOCAL_CFLAGS := -D_REENTRANT -DLINUX -DANDROID
+LOCAL_CFLAGS += -Wall -Werror
+
+LOCAL_C_INCLUDES := \
+	$(MLSDK_PATH)/platform/include \
+	$(MLSDK_PATH)/platform/include/linux \
+	$(MLSDK_PATH)/platform/linux \
+	$(MLSDK_PATH)/platform/linux/kernel \
+	$(MLSDK_PATH)/mllite
+
+LOCAL_SRC_FILES := \
+	mlsdk/platform/linux/mlos_linux.c \
+	mlsdk/platform/linux/mlsl_linux_mpu.c
+
+LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils
+include $(BUILD_SHARED_LIBRARY)
+
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_CFLAGS := -DNDEBUG -D_REENTRANT -DLINUX -DANDROID
+LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE
+LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\"
+LOCAL_CFLAGS += -Wall -Werror
+
+# 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
+
+LOCAL_C_INCLUDES := \
+	$(MLSDK_PATH)/mldmp \
+	$(MLSDK_PATH)/mllite \
+	$(MLSDK_PATH)/platform/include \
+	$(MLSDK_PATH)/platform/include/linux \
+	$(MLSDK_PATH)/mlutils \
+	$(MLSDK_PATH)/mlapps/common \
+	$(MLSDK_PATH)/mllite/akmd \
+	$(MLSDK_PATH)/platform/linux
+
+LOCAL_SRC_FILES := \
+	mlsdk/mllite/accel.c \
+	mlsdk/mllite/compass.c \
+	mlsdk/mllite/pressure.c \
+	mlsdk/mllite/mldl_cfg_mpu.c \
+	mlsdk/mllite/dmpDefault.c \
+	mlsdk/mllite/ml.c \
+	mlsdk/mllite/mlarray.c \
+	mlsdk/mllite/mlarray_legacy.c \
+	mlsdk/mllite/mlFIFO.c \
+	mlsdk/mllite/mlFIFOHW.c \
+	mlsdk/mllite/mlMathFunc.c \
+	mlsdk/mllite/ml_stored_data.c \
+	mlsdk/mllite/mlcontrol.c \
+	mlsdk/mllite/mldl.c \
+	mlsdk/mllite/mldmp.c \
+	mlsdk/mllite/mlstates.c \
+	mlsdk/mllite/mlsupervisor.c \
+	mlsdk/mllite/mlBiasNoMotion.c \
+	mlsdk/mllite/mlSetGyroBias.c \
+	mlsdk/mllite/ml_mputest.c \
+	mlsdk/mlutils/mputest.c \
+	mlsdk/mlutils/checksum.c
+
+LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform
+include $(BUILD_SHARED_LIBRARY)
diff --git a/libsensors/mlsdk/mllite/accel.c b/libsensors/mlsdk/mllite/accel.c
new file mode 100644
index 0000000..b8a0ed2
--- /dev/null
+++ b/libsensors/mlsdk/mllite/accel.c
@@ -0,0 +1,189 @@
+/*
+ $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];
+    unsigned 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/libsensors/mlsdk/mllite/accel.h b/libsensors/mlsdk/mllite/accel.h
new file mode 100644
index 0000000..d3fbc6a
--- /dev/null
+++ b/libsensors/mlsdk/mllite/accel.h
@@ -0,0 +1,57 @@
+/*
+ $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/libsensors/mlsdk/mllite/compass.c b/libsensors/mlsdk/mllite/compass.c
new file mode 100644
index 0000000..c008fbf
--- /dev/null
+++ b/libsensors/mlsdk/mllite/compass.c
@@ -0,0 +1,538 @@
+/*
+ $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 {
+        return INV_ERROR_INVALID_CONFIGURATION;
+    }
+    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/libsensors/mlsdk/mllite/compass.h b/libsensors/mlsdk/mllite/compass.h
new file mode 100644
index 0000000..f0bdb58
--- /dev/null
+++ b/libsensors/mlsdk/mllite/compass.h
@@ -0,0 +1,92 @@
+/*
+ $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/libsensors/mlsdk/mllite/dmpDefault.c b/libsensors/mlsdk/mllite/dmpDefault.c
new file mode 100644
index 0000000..b649c0d
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpDefault.c
@@ -0,0 +1,418 @@
+/*
+ $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"
+#include "accel.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 ) {
+        unsigned short 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;
+    }
+
+    if (inv_accel_present())
+    {
+        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/libsensors/mlsdk/mllite/dmpDefault.h b/libsensors/mlsdk/mllite/dmpDefault.h
new file mode 100644
index 0000000..1fc7ca6
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpDefault.h
@@ -0,0 +1,42 @@
+/*
+ $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/libsensors/mlsdk/mllite/dmpDefaultMantis.c b/libsensors/mlsdk/mllite/dmpDefaultMantis.c
new file mode 100644
index 0000000..5282dd9
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpDefaultMantis.c
@@ -0,0 +1,467 @@
+/*
+ $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/libsensors/mlsdk/mllite/dmpKey.h b/libsensors/mlsdk/mllite/dmpKey.h
new file mode 100644
index 0000000..3e2e667
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpKey.h
@@ -0,0 +1,398 @@
+/*
+ $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
+
+#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
+
+
+#endif // DMPKEY_H__
diff --git a/libsensors/mlsdk/mllite/dmpconfig.txt b/libsensors/mlsdk/mllite/dmpconfig.txt
new file mode 100644
index 0000000..4643ed5
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpconfig.txt
@@ -0,0 +1,3 @@
+major version    =  1
+minor version    =  0
+startAddr        =  0
diff --git a/libsensors/mlsdk/mllite/dmpmap.h b/libsensors/mlsdk/mllite/dmpmap.h
new file mode 100644
index 0000000..cb53c7c
--- /dev/null
+++ b/libsensors/mlsdk/mllite/dmpmap.h
@@ -0,0 +1,276 @@
+/*
+ $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/libsensors/mlsdk/mllite/invensense.h b/libsensors/mlsdk/mllite/invensense.h
new file mode 100644
index 0000000..586dd25
--- /dev/null
+++ b/libsensors/mlsdk/mllite/invensense.h
@@ -0,0 +1,24 @@
+/** 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/libsensors/mlsdk/mllite/ml.c b/libsensors/mlsdk/mllite/ml.c
new file mode 100644
index 0000000..d830be7
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml.c
@@ -0,0 +1,1791 @@
+/*
+ $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 "mlSetGyroBias.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];
+        if (NULL != mldl_cfg->accel){
+            accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii];
+        }
+        if (NULL != mldl_cfg->compass){
+            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;
+    }
+
+    if (NULL != mldl_cfg->accel){
+        RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale);
+        inv_obj.accel_sens = (long)(accelScale * 65536L);
+        /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */
+        inv_obj.accel_sens /= 2;
+    }
+    if (NULL != mldl_cfg->compass){
+        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;
+        }
+        if (NULL != mldl_cfg->accel){
+            result = inv_set_accel_calibration(accelScale, accelCal);
+            if (INV_SUCCESS != result) {
+                MPL_LOGE("Unable to set Accel Calibration\n");
+                return result;
+            }
+        }
+        if (NULL != mldl_cfg->compass){
+            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();
+
+    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;
+    }
+    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) {
+        unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
+        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;
+        regs[2] = DINA66;
+
+        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));
+    }
+
+    {
+        unsigned char tmpD = DINAC9;
+        unsigned char tmpE = DINA2C;
+        unsigned char tmpF = DINACB;
+        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 (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
+            reg = 0x2;
+        } else {
+            reg = 0;
+        }
+    }
+
+    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;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors/mlsdk/mllite/ml.h b/libsensors/mlsdk/mllite/ml.h
new file mode 100644
index 0000000..67e47e2
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml.h
@@ -0,0 +1,589 @@
+/*
+ $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);
+
+    /* 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/libsensors/mlsdk/mllite/mlBiasNoMotion.c b/libsensors/mlsdk/mllite/mlBiasNoMotion.c
new file mode 100644
index 0000000..aaf98d2
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlBiasNoMotion.c
@@ -0,0 +1,307 @@
+/*
+ $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;
+}
+
+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 (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();
+            }
+        }
+    }
+
+    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);
+    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);
+    return result;
+}
diff --git a/libsensors/mlsdk/mllite/mlBiasNoMotion.h b/libsensors/mlsdk/mllite/mlBiasNoMotion.h
new file mode 100644
index 0000000..030dbf9
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlBiasNoMotion.h
@@ -0,0 +1,40 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlFIFO.c b/libsensors/mlsdk/mllite/mlFIFO.c
new file mode 100644
index 0000000..3279b35
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlFIFO.c
@@ -0,0 +1,2126 @@
+/*
+ $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"
+#include "mpu3050.h"
+#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;
+    unsigned 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)
+{
+    // Celcius = 35 + (T + 13200)/280
+    return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L);
+}
+
+/**  @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 __unused)
+{
+    memset(data, 0, COMPASS_NUM_AXES * sizeof(long));
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  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 __unused, 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 __unused,
+                                           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;
+    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();
+}
+
+/** 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 __unused,
+                                          uint_fast16_t accuracy __unused)
+{
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;    // Feature not supported
+}
+
+/**
+ *  @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/libsensors/mlsdk/mllite/mlFIFO.h b/libsensors/mlsdk/mllite/mlFIFO.h
new file mode 100644
index 0000000..2114eb3
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlFIFO.h
@@ -0,0 +1,150 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlFIFOHW.c b/libsensors/mlsdk/mllite/mlFIFOHW.c
new file mode 100644
index 0000000..e806b95
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlFIFOHW.c
@@ -0,0 +1,320 @@
+/*
+ $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"
+#include "mpu3050.h"
+#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/libsensors/mlsdk/mllite/mlFIFOHW.h b/libsensors/mlsdk/mllite/mlFIFOHW.h
new file mode 100644
index 0000000..6f70185
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlFIFOHW.h
@@ -0,0 +1,48 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlMathFunc.c b/libsensors/mlsdk/mllite/mlMathFunc.c
new file mode 100644
index 0000000..31b42bc
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlMathFunc.c
@@ -0,0 +1,377 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlMathFunc.h b/libsensors/mlsdk/mllite/mlMathFunc.h
new file mode 100644
index 0000000..70fa9f4
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlMathFunc.h
@@ -0,0 +1,68 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlSetGyroBias.c b/libsensors/mlsdk/mllite/mlSetGyroBias.c
new file mode 100644
index 0000000..bd14d2e
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlSetGyroBias.c
@@ -0,0 +1,198 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlSetGyroBias.h b/libsensors/mlsdk/mllite/mlSetGyroBias.h
new file mode 100644
index 0000000..e56f18b
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlSetGyroBias.h
@@ -0,0 +1,49 @@
+/*
+ $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/libsensors/mlsdk/mllite/ml_mputest.c b/libsensors/mlsdk/mllite/ml_mputest.c
new file mode 100644
index 0000000..ffb17cd
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml_mputest.c
@@ -0,0 +1,180 @@
+/*
+ $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)
+{
+    return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE);
+}
+
+/**
+ *  @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/libsensors/mlsdk/mllite/ml_mputest.h b/libsensors/mlsdk/mllite/ml_mputest.h
new file mode 100644
index 0000000..705d3cc
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml_mputest.h
@@ -0,0 +1,49 @@
+/*
+ $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/libsensors/mlsdk/mllite/ml_stored_data.c b/libsensors/mlsdk/mllite/ml_stored_data.c
new file mode 100644
index 0000000..35da3e8
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml_stored_data.c
@@ -0,0 +1,1586 @@
+/*
+ $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/libsensors/mlsdk/mllite/ml_stored_data.h b/libsensors/mlsdk/mllite/ml_stored_data.h
new file mode 100644
index 0000000..74c2b7c
--- /dev/null
+++ b/libsensors/mlsdk/mllite/ml_stored_data.h
@@ -0,0 +1,62 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlarray.c b/libsensors/mlsdk/mllite/mlarray.c
new file mode 100644
index 0000000..2ee9a8c
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlarray.c
@@ -0,0 +1,2524 @@
+/*
+ $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 __unused)
+{
+
+    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 __unused)
+{
+    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/libsensors/mlsdk/mllite/mlarray_legacy.c b/libsensors/mlsdk/mllite/mlarray_legacy.c
new file mode 100644
index 0000000..20d9116
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlarray_legacy.c
@@ -0,0 +1,587 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlcontrol.c b/libsensors/mlsdk/mllite/mlcontrol.c
new file mode 100644
index 0000000..278438a
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlcontrol.c
@@ -0,0 +1,797 @@
+/*
+ $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 __unused)
+{
+    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/libsensors/mlsdk/mllite/mlcontrol.h b/libsensors/mlsdk/mllite/mlcontrol.h
new file mode 100644
index 0000000..abdefa3
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlcontrol.h
@@ -0,0 +1,217 @@
+/*
+ $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/libsensors/mlsdk/mllite/mldl.c b/libsensors/mlsdk/mllite/mldl.c
new file mode 100644
index 0000000..ee7258f
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldl.c
@@ -0,0 +1,1051 @@
+/*
+ $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"
+#include "mpu3050.h"
+#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;
+
+    /* 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;
+    }
+
+    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)
+{
+    unsigned 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) {
+        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 {
+        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];
+    unsigned 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/libsensors/mlsdk/mllite/mldl.h b/libsensors/mlsdk/mllite/mldl.h
new file mode 100644
index 0000000..961d860
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldl.h
@@ -0,0 +1,176 @@
+/*
+ $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/libsensors/mlsdk/mllite/mldl_cfg.h b/libsensors/mlsdk/mllite/mldl_cfg.h
new file mode 100644
index 0000000..fb2e402
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldl_cfg.h
@@ -0,0 +1,324 @@
+/*
+ $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>
+#include "mpu3050.h"
+
+#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;
+
+	/* 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/libsensors/mlsdk/mllite/mldl_cfg_mpu.c b/libsensors/mlsdk/mllite/mldl_cfg_mpu.c
new file mode 100644
index 0000000..ff97348
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldl_cfg_mpu.c
@@ -0,0 +1,474 @@
+/*
+ $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 (mldl_cfg->accel) {
+        MPL_LOGD("slave_accel->suspend      = %p\n", mldl_cfg->accel->suspend);
+        MPL_LOGD("slave_accel->resume       = %p\n", mldl_cfg->accel->resume);
+        MPL_LOGD("slave_accel->read         = %p\n", 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    = %p\n", mldl_cfg->compass->suspend);
+        MPL_LOGD("slave_compass->resume     = %p\n", mldl_cfg->compass->resume);
+        MPL_LOGD("slave_compass->read       = %p\n", 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    = %p\n", mldl_cfg->pressure->suspend);
+        MPL_LOGD("slave_pressure->resume     = %p\n", mldl_cfg->pressure->resume);
+        MPL_LOGD("slave_pressure->read       = %p\n", 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    = %p\n",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  = %p\n",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  = %p\n",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: %zu, "
+             "ext_slave_descr:%zu, mpu_platform_data:%zu: RamOffset: %zu\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 __unused,
+                 void *compass_handle __unused,
+                 void *pressure_handle __unused)
+{
+    int result;
+    result = ioctl((int)(uintptr_t)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 __unused,
+		  void *compass_handle __unused,
+		  void *pressure_handle __unused)
+{
+    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 __unused,
+                   void *compass_handle __unused,
+                   void *pressure_handle __unused,
+                   unsigned long sensors)
+{
+    int result;
+
+    mldl_cfg->requested_sensors = sensors;
+    result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = ioctl((int)(uintptr_t)mlsl_handle, MPU_RESUME, NULL);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = ioctl((int)(uintptr_t)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 __unused,
+                    void *compass_handle __unused,
+                    void *pressure_handle __unused,
+                    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)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SUSPEND, NULL);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = ioctl((int)(uintptr_t)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 __unused,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata __unused,
+		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)(uintptr_t)gyro_handle, MPU_READ_ACCEL, data);
+        break;
+    case EXT_SLAVE_TYPE_COMPASS:
+        result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_COMPASS, data);
+        break;
+    case EXT_SLAVE_TYPE_PRESSURE:
+        result = ioctl((int)(uintptr_t)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 __unused,
+                     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)(uintptr_t)gyro_handle, MPU_CONFIG_ACCEL, data);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        break;
+    case EXT_SLAVE_TYPE_COMPASS:
+        result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_COMPASS, data);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        break;
+    case EXT_SLAVE_TYPE_PRESSURE:
+        result = ioctl((int)(uintptr_t)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 __unused,
+                         struct ext_slave_config *data,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata __unused)
+{
+    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)(uintptr_t)gyro_handle, MPU_GET_CONFIG_ACCEL, data);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        break;
+    case EXT_SLAVE_TYPE_COMPASS:
+        result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_COMPASS, data);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        break;
+    case EXT_SLAVE_TYPE_PRESSURE:
+        result = ioctl((int)(uintptr_t)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/libsensors/mlsdk/mllite/mldmp.c b/libsensors/mlsdk/mllite/mldmp.c
new file mode 100644
index 0000000..d16bdaf
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldmp.c
@@ -0,0 +1,284 @@
+/*
+ $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;
+    }
+    if (NULL != mldl_cfg->accel){
+        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/libsensors/mlsdk/mllite/mldmp.h b/libsensors/mlsdk/mllite/mldmp.h
new file mode 100644
index 0000000..ff3d24e
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mldmp.h
@@ -0,0 +1,96 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlinclude.h b/libsensors/mlsdk/mllite/mlinclude.h
new file mode 100644
index 0000000..fdbdef3
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlinclude.h
@@ -0,0 +1,50 @@
+/*
+ $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 INV_INCLUDE_H__
+#define INV_INCLUDE_H__
+
+#define INVENSENSE_FUNC_START
+
+#ifdef COVERAGE
+#include "utestCommon.h"
+#endif
+#ifdef PROFILE
+#include "profile.h"
+#endif
+
+#ifdef WIN32
+#ifdef COVERAGE
+
+extern int functionEnterLog(const char *file, const char *func);
+extern int functionExitLog(const char *file, const char *func);
+
+#undef INVENSENSE_FUNC_START
+#define INVENSENSE_FUNC_START  __pragma(message(__FILE__ "|"__FUNCTION__ )) \
+    int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__)
+#endif // COVERAGE
+#endif // WIN32
+
+#ifdef PROFILE
+#undef INVENSENSE_FUNC_START
+#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__)
+#define return if ( profileExit(__FILE__, __FUNCTION__) ) return
+#endif // PROFILE
+
+// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return
+
+#endif //INV_INCLUDE_H__
diff --git a/libsensors/mlsdk/mllite/mlstates.c b/libsensors/mlsdk/mllite/mlstates.c
new file mode 100644
index 0000000..e44f100
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlstates.c
@@ -0,0 +1,269 @@
+/*
+ $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, 0, { NULL } };
+
+/* --------------- */
+/* -  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/libsensors/mlsdk/mllite/mlstates.h b/libsensors/mlsdk/mllite/mlstates.h
new file mode 100644
index 0000000..cbaa610
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlstates.h
@@ -0,0 +1,58 @@
+/*
+ $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/libsensors/mlsdk/mllite/mlsupervisor.c b/libsensors/mlsdk/mllite/mlsupervisor.c
new file mode 100644
index 0000000..2546903
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlsupervisor.c
@@ -0,0 +1,587 @@
+/*
+ $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 = { NULL, NULL, NULL, NULL, NULL };
+
+/**
+ *  @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 (ml_supervisor_cb.supervisor_reset_func != NULL) {
+        ml_supervisor_cb.supervisor_reset_func();
+    }
+}
+
+static int MLUpdateCompassCalibration3DOF(int command, long *data,
+                                          unsigned long deltaTime __unused)
+{
+    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/libsensors/mlsdk/mllite/mlsupervisor.h b/libsensors/mlsdk/mllite/mlsupervisor.h
new file mode 100644
index 0000000..62b427e
--- /dev/null
+++ b/libsensors/mlsdk/mllite/mlsupervisor.h
@@ -0,0 +1,71 @@
+/*
+ $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/libsensors/mlsdk/mllite/pressure.c b/libsensors/mlsdk/mllite/pressure.c
new file mode 100644
index 0000000..f32229f
--- /dev/null
+++ b/libsensors/mlsdk/mllite/pressure.c
@@ -0,0 +1,166 @@
+/*
+ $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/libsensors/mlsdk/mllite/pressure.h b/libsensors/mlsdk/mllite/pressure.h
new file mode 100644
index 0000000..77c5d43
--- /dev/null
+++ b/libsensors/mlsdk/mllite/pressure.h
@@ -0,0 +1,71 @@
+/*
+ $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/libsensors/mlsdk/mlutils/checksum.c b/libsensors/mlsdk/mlutils/checksum.c
new file mode 100644
index 0000000..a97477d
--- /dev/null
+++ b/libsensors/mlsdk/mlutils/checksum.c
@@ -0,0 +1,16 @@
+#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/libsensors/mlsdk/mlutils/checksum.h b/libsensors/mlsdk/mlutils/checksum.h
new file mode 100644
index 0000000..4d3f046
--- /dev/null
+++ b/libsensors/mlsdk/mlutils/checksum.h
@@ -0,0 +1,18 @@
+#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/libsensors/mlsdk/mlutils/mputest.c b/libsensors/mlsdk/mlutils/mputest.c
new file mode 100644
index 0000000..4a598ff
--- /dev/null
+++ b/libsensors/mlsdk/mlutils/mputest.c
@@ -0,0 +1,1101 @@
+/*
+ $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)
+
+/**
+ *  @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;
+}
+
+#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);
+
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        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 */
+        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;
+            }
+        }
+
+        inv_sleep(60);
+    } else {
+        /* restore the power state the part was found in */
+        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;
+            }
+        }
+    }
+
+    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 */
+    result = inv_test_gyro_3050(mlsl_handle,
+                gyro_biases, &temp_avg, provide_result);
+
+    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);
+    } else {
+        /* would be 0, but 1 to avoid divide-by-0 below */
+        accelSens[0] = accelSens[1] = accelSens[2] = 1;
+    }
+    result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
+                            provide_result);
+    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;
+
+    result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23,
+                             6, data);
+    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/libsensors/mlsdk/mlutils/mputest.h b/libsensors/mlsdk/mlutils/mputest.h
new file mode 100644
index 0000000..d3347c5
--- /dev/null
+++ b/libsensors/mlsdk/mlutils/mputest.h
@@ -0,0 +1,54 @@
+/*
+ $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/libsensors/mlsdk/mlutils/slave.h b/libsensors/mlsdk/mlutils/slave.h
new file mode 100644
index 0000000..45449f6
--- /dev/null
+++ b/libsensors/mlsdk/mlutils/slave.h
@@ -0,0 +1,188 @@
+/*
+ $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: slave.h 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef SLAVE_H
+#define SLAVE_H
+
+/**
+ *  @addtogroup SLAVEDL
+ *
+ *  @{
+ *      @file     slave.h
+ *      @brief    Top level descriptions for Accelerometer support
+ *
+ */
+
+#include "mltypes.h"
+#include "mpu.h"
+
+    /* ------------ */
+    /* - Defines. - */
+    /* ------------ */
+
+/*--- default accel support - selection ---*/
+#define ACCEL_ST_LIS331                 0
+#define ACCEL_KIONIX_KXTF9              1
+#define ACCEL_BOSCH                     0
+#define ACCEL_ADI                       0
+
+#define ACCEL_SLAVEADDR_INVALID         0x00
+
+#define ACCEL_SLAVEADDR_LIS331          0x18
+#define ACCEL_SLAVEADDR_LSM303          0x18
+#define ACCEL_SLAVEADDR_LIS3DH          0x18
+#define ACCEL_SLAVEADDR_KXSD9           0x18
+#define ACCEL_SLAVEADDR_KXTF9           0x0F
+#define ACCEL_SLAVEADDR_BMA150          0x38
+#define ACCEL_SLAVEADDR_BMA222          0x08
+#define ACCEL_SLAVEADDR_BMA250          0x18
+#define ACCEL_SLAVEADDR_ADXL34X         0x53
+#define ACCEL_SLAVEADDR_ADXL34X_ALT     0x1D /* alternative addr */
+#define ACCEL_SLAVEADDR_MMA8450         0x1C
+#define ACCEL_SLAVEADDR_MMA845X         0x1C
+
+#define ACCEL_SLAVEADDR_INVENSENSE      0x68
+/*
+    Define default accelerometer to use if no selection is made
+*/
+#if ACCEL_ST_LIS331
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_LIS331
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_LIS331
+#endif
+
+#if ACCEL_ST_LSM303
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_LSM303
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_LSM303A
+#endif
+
+#if ACCEL_KIONIX_KXSD9
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_KXSD9
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_KXSD9
+#endif
+
+#if ACCEL_KIONIX_KXTF9
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_KXTF9
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_KXTF9
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA150
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA150
+#endif
+
+#if ACCEL_BMA222
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA222
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA222
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_BMA250
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_BMA250
+#endif
+
+#if ACCEL_ADI
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_ADXL34X
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_ADXL34X
+#endif
+
+#if ACCEL_MMA8450
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_MMA8450
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_MMA8450
+#endif
+
+#if ACCEL_MMA845X
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_MMA845X
+#define DEFAULT_ACCEL_ID                  ACCEL_ID_MMA845X
+#endif
+
+/*--- if no default accelerometer was selected ---*/
+#ifndef DEFAULT_ACCEL_SLAVEADDR
+#define DEFAULT_ACCEL_SLAVEADDR           ACCEL_SLAVEADDR_INVALID
+#endif
+
+#define USE_COMPASS_AICHI                  0
+#define USE_COMPASS_AKM                    0
+#define USE_COMPASS_YAS529                 0
+#define USE_COMPASS_YAS530                 0
+#define USE_COMPASS_HMC5883                0
+#define USE_COMPASS_MMC314X                0
+#define USE_COMPASS_HSCDTD002B             0
+#define USE_COMPASS_HSCDTD004A             0
+
+#define COMPASS_SLAVEADDR_INVALID          0x00
+#define COMPASS_SLAVEADDR_AKM_BASE         0x0C
+#define COMPASS_SLAVEADDR_AKM              0x0E
+#define COMPASS_SLAVEADDR_AMI304           0x0E
+#define COMPASS_SLAVEADDR_AMI305           0x0F /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_AMI306           0x0E /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_YAS529           0x2E
+#define COMPASS_SLAVEADDR_YAS530           0x2E
+#define COMPASS_SLAVEADDR_HMC5883          0x1E
+#define COMPASS_SLAVEADDR_MMC314X          0x30
+#define COMPASS_SLAVEADDR_HSCDTD00XX       0x0C
+
+/*
+    Define default compass to use if no selection is made
+*/
+ #if USE_COMPASS_AKM
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_AK8975
+ #endif
+
+ #if USE_COMPASS_AICHI
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_AMI30X
+ #endif
+
+ #if USE_COMPASS_YAS529
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_YAS529
+ #endif
+
+ #if USE_COMPASS_YAS530
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_YAS530
+ #endif
+
+ #if USE_COMPASS_HMC5883
+ #define DEFAULT_COMPASS_TYPE              COMPASS_ID_HMC5883
+ #endif
+
+#if USE_COMPASS_MMC314X
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_MMC314X
+#endif
+
+#if USE_COMPASS_HSCDTD002B
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_HSCDTD002B
+#endif
+
+#if USE_COMPASS_HSCDTD004A
+#define DEFAULT_COMPASS_TYPE              COMPASS_ID_HSCDTD004A
+#endif
+
+#ifndef DEFAULT_COMPASS_TYPE
+#define DEFAULT_COMPASS_TYPE               ID_INVALID
+#endif
+
+
+#endif // SLAVE_H
+
+/**
+ *  @}
+ */
diff --git a/libsensors/mlsdk/platform/include/i2c.h b/libsensors/mlsdk/platform/include/i2c.h
new file mode 100644
index 0000000..c3002d5
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/i2c.h
@@ -0,0 +1,133 @@
+/*
+ $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/libsensors/mlsdk/platform/include/linux/mpu.h b/libsensors/mlsdk/platform/include/linux/mpu.h
new file mode 100644
index 0000000..04fa7b6
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/linux/mpu.h
@@ -0,0 +1,335 @@
+/*
+ $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_
+#define __MPU_H_
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#include <linux/ioctl.h>
+#elif defined LINUX
+#include <sys/ioctl.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;
+};
+
+enum mpuirq_data_type {
+	MPUIRQ_DATA_TYPE_MPU_IRQ,
+	MPUIRQ_DATA_TYPE_SLAVE_IRQ,
+	MPUIRQ_DATA_TYPE_PM_EVENT,
+	MPUIRQ_DATA_TYPE_NUM_TYPES,
+};
+
+/* User space PM event notification */
+#define MPU_PM_EVENT_SUSPEND_PREPARE (3)
+#define MPU_PM_EVENT_POST_SUSPEND    (4)
+
+struct mpuirq_data {
+	int interruptcount;
+	unsigned long long irqtime;
+	int data_type;
+	long data;
+};
+
+enum ext_slave_config_key {
+	MPU_SLAVE_CONFIG_ODR_SUSPEND,
+	MPU_SLAVE_CONFIG_ODR_RESUME,
+	MPU_SLAVE_CONFIG_FSR_SUSPEND,
+	MPU_SLAVE_CONFIG_FSR_RESUME,
+	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_WRITE_REGISTERS,
+	MPU_SLAVE_READ_REGISTERS,
+	/* 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,
+};
+
+/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */
+enum ext_slave_config_irq_type {
+	MPU_SLAVE_IRQ_TYPE_NONE,
+	MPU_SLAVE_IRQ_TYPE_MOTION,
+	MPU_SLAVE_IRQ_TYPE_DATA_READY,
+};
+
+/* Structure for the following IOCTS's
+ * MPU_CONFIG_ACCEL
+ * MPU_CONFIG_COMPASS
+ * MPU_CONFIG_PRESSURE
+ * MPU_GET_CONFIG_ACCEL
+ * MPU_GET_CONFIG_COMPASS
+ * MPU_GET_CONFIG_PRESSURE
+ *
+ * @key one of enum ext_slave_config_key
+ * @len length of data pointed to by data
+ * @apply zero if communication with the chip is not necessary, false otherwise
+ *        This flag can be used to select cached data or to refresh cashed data
+ *        cache data to be pushed later or push immediately.  If true and the
+ *        slave is on the secondary bus the MPU will first enger bypass mode
+ *        before calling the slaves .config or .get_config funcion
+ * @data pointer to the data to confgure or get
+ */
+struct ext_slave_config {
+	int key;
+	int len;
+	int apply;
+	void *data;
+};
+
+enum ext_slave_type {
+	EXT_SLAVE_TYPE_GYROSCOPE,
+	EXT_SLAVE_TYPE_ACCELEROMETER,
+	EXT_SLAVE_TYPE_COMPASS,
+	EXT_SLAVE_TYPE_PRESSURE,
+	/*EXT_SLAVE_TYPE_TEMPERATURE */
+
+	EXT_SLAVE_NUM_TYPES
+};
+
+enum ext_slave_id {
+	ID_INVALID = 0,
+
+	ACCEL_ID_LIS331,
+	ACCEL_ID_LSM303A,
+	ACCEL_ID_LIS3DH,
+	ACCEL_ID_KXSD9,
+	ACCEL_ID_KXTF9,
+	ACCEL_ID_BMA150,
+	ACCEL_ID_BMA222,
+	ACCEL_ID_BMA250,
+	ACCEL_ID_ADXL34X,
+	ACCEL_ID_MMA8450,
+	ACCEL_ID_MMA845X,
+	ACCEL_ID_MPU6050,
+
+	COMPASS_ID_AK8975,
+	COMPASS_ID_AMI30X,
+	COMPASS_ID_AMI306,
+	COMPASS_ID_YAS529,
+	COMPASS_ID_YAS530,
+	COMPASS_ID_HMC5883,
+	COMPASS_ID_LSM303M,
+	COMPASS_ID_MMC314X,
+	COMPASS_ID_HSCDTD002B,
+	COMPASS_ID_HSCDTD004A,
+
+	PRESSURE_ID_BMA085,
+};
+
+enum ext_slave_endian {
+	EXT_SLAVE_BIG_ENDIAN,
+	EXT_SLAVE_LITTLE_ENDIAN,
+	EXT_SLAVE_FS8_BIG_ENDIAN,
+	EXT_SLAVE_FS16_BIG_ENDIAN,
+};
+
+enum ext_slave_bus {
+	EXT_SLAVE_BUS_INVALID = -1,
+	EXT_SLAVE_BUS_PRIMARY = 0,
+	EXT_SLAVE_BUS_SECONDARY = 1
+};
+
+
+/**
+ *  struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
+ *  slave devices
+ *
+ *  @get_slave_descr: Function pointer to retrieve the struct ext_slave_descr
+ *                    for this slave
+ *  @irq: the irq number attached to the slave if any.
+ *  @adapt_num: the I2C adapter number.
+ *  @bus: the bus the slave is attached to: enum ext_slave_bus
+ *  @address: the I2C slave address of the slave device.
+ *  @orientation: the mounting matrix of the device relative to MPU.
+ *  @irq_data: private data for the slave irq handler
+ *  @private_data: additional data, user customizable.  Not touched by the MPU
+ *                 driver.
+ *
+ * The orientation matricies are 3x3 rotation matricies
+ * that are applied to the data to rotate from the mounting orientation to the
+ * platform orientation.  The values must be one of 0, 1, or -1 and each row and
+ * column should have exactly 1 non-zero value.
+ */
+struct ext_slave_platform_data {
+	struct ext_slave_descr *(*get_slave_descr) (void);
+	int irq;
+	int adapt_num;
+	int bus;
+	unsigned char address;
+	signed char orientation[9];
+	void *irq_data;
+	void *private_data;
+};
+
+struct fix_pnt_range {
+	long mantissa;
+	long fraction;
+};
+
+static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
+{
+	return (long)(rng.mantissa * 1000 + rng.fraction / 10);
+}
+
+struct ext_slave_read_trigger {
+	unsigned char reg;
+	unsigned char value;
+};
+
+/**
+ *  struct ext_slave_descr - Description of the slave device for programming.
+ *
+ *  @suspend:	function pointer to put the device in suspended state
+ *  @resume:	function pointer to put the device in running state
+ *  @read:	function that reads the device data
+ *  @init:	function used to preallocate memory used by the driver
+ *  @exit:	function used to free memory allocated for the driver
+ *  @config:	function used to configure the device
+ *  @get_config:function used to get the device's configuration
+ *
+ *  @name:	text name of the device
+ *  @type:	device type. enum ext_slave_type
+ *  @id:	enum ext_slave_id
+ *  @reg:	starting register address to retrieve data.
+ *  @len:	length in bytes of the sensor data.  Should be 6.
+ *  @endian:	byte order of the data. enum ext_slave_endian
+ *  @range:	full scale range of the slave ouput: struct fix_pnt_range
+ *  @trigger:	If reading data first requires writing a register this is the
+ *		data to write.
+ *
+ *  Defines the functions and information about the slave the mpu3050 and
+ *  mpu6050 needs to use the slave device.
+ */
+struct ext_slave_descr {
+	int (*init) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata);
+	int (*exit) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata);
+	int (*suspend) (void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata);
+	int (*resume) (void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata);
+	int (*read) (void *mlsl_handle,
+		     struct ext_slave_descr *slave,
+		     struct ext_slave_platform_data *pdata,
+		     unsigned char *data);
+	int (*config) (void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       struct ext_slave_config *config);
+	int (*get_config) (void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata,
+			   struct ext_slave_config *config);
+
+	char *name;
+	unsigned char type;
+	unsigned char id;
+	unsigned char read_reg;
+	unsigned int read_len;
+	unsigned char endian;
+	struct fix_pnt_range range;
+	struct ext_slave_read_trigger *trigger;
+};
+
+/**
+ * struct mpu_platform_data - Platform data for the mpu driver
+ * @int_config:		Bits [7:3] of the int config register.
+ * @orientation:	Orientation matrix of the gyroscope
+ * @level_shifter:	0: VLogic, 1: VDD
+ * @accel:		Accel platform data
+ * @compass:		Compass platform data
+ * @pressure:		Pressure platform data
+ *
+ * Contains platform specific information on how to configure the MPU3050 to
+ * work on this platform.  The orientation matricies are 3x3 rotation matricies
+ * that are applied to the data to rotate from the mounting orientation to the
+ * platform orientation.  The values must be one of 0, 1, or -1 and each row and
+ * column should have exactly 1 non-zero value.
+ */
+struct 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;
+};
+
+#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_ */
diff --git a/libsensors/mlsdk/platform/include/log.h b/libsensors/mlsdk/platform/include/log.h
new file mode 100644
index 0000000..8485074
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/log.h
@@ -0,0 +1,344 @@
+/*
+ $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.
+  $
+ */
+
+/*
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ * Copyright (C) 2005 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.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include "mltypes.h"
+#include <stdarg.h>
+
+#ifdef ANDROID
+#ifdef NDK_BUILD
+#include "log_macros.h"
+#else
+#include <utils/Log.h>		/* For the LOG macro */
+#endif
+#endif
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#ifdef __KERNEL__
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+#else
+	/* Based off the log priorities in android
+	   /system/core/include/android/log.h */
+#define MPL_LOG_UNKNOWN		(0)
+#define MPL_LOG_DEFAULT		(1)
+#define MPL_LOG_VERBOSE		(2)
+#define MPL_LOG_DEBUG		(3)
+#define MPL_LOG_INFO		(4)
+#define MPL_LOG_WARN		(5)
+#define MPL_LOG_ERROR		(6)
+#define MPL_LOG_SILENT		(8)
+#endif
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#ifdef __KERNEL__
+#define MPL_LOG_TAG
+#else
+#define MPL_LOG_TAG NULL
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+	} while (0)
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+	do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+	((CONDITION(cond))						\
+		? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+		: (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((CONDITION(cond))                                              \
+		? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#ifdef __KERNEL__
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#ifdef __KERNEL__
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+						fmt, ##__VA_ARGS__))	\
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)				\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+	} while (0)
+#define MPL_LOG_FATAL(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)	\
+	} while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)			\
+	MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)		\
+	MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#ifdef ANDROID
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+        ALOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	_MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+	_MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+int _MLPrintLog(int priority, const char *tag, const char *fmt,	...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+static inline void __print_result_location(int result,
+					   const char *file,
+					   const char *func, int line)
+{
+	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#define LOG_RESULT_LOCATION(condition) \
+	do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/libsensors/mlsdk/platform/include/mlmath.h b/libsensors/mlsdk/platform/include/mlmath.h
new file mode 100644
index 0000000..424a43b
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/mlmath.h
@@ -0,0 +1,107 @@
+/*
+ $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: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef _ML_MATH_H_
+#define	_ML_MATH_H_
+
+#ifndef MLMATH
+// This define makes Microsoft pickup things like M_PI
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#ifdef WIN32
+// Microsoft doesn't follow standards
+#define  round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5))))
+#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
+#endif
+
+#else  // MLMATH
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/* MPL needs below functions */
+double	ml_asin(double);
+double	ml_atan(double);
+double	ml_atan2(double, double);
+double	ml_log(double);
+double	ml_sqrt(double);
+double	ml_ceil(double);
+double	ml_floor(double);
+double  ml_cos(double);
+double  ml_sin(double);
+double  ml_acos(double);
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+/*
+ * We rename functions here to provide the hook for other
+ * customized math functions.
+ */
+#define	sqrt(x)      ml_sqrt(x)
+#define	log(x)       ml_log(x)
+#define	asin(x)      ml_asin(x)
+#define	atan(x)      ml_atan(x)
+#define	atan2(x,y)   ml_atan2(x,y)
+#define	ceil(x)      ml_ceil(x)
+#define	floor(x)     ml_floor(x)
+#define fabs(x)      (((x)<0)?-(x):(x))
+#define round(x)     (((double)((long long)((x)>0?(x)+.5:(x)-.5))))
+#define roundf(x)    (((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
+#define cos(x)       ml_cos(x)
+#define sin(x)       ml_sin(x)
+#define acos(x)      ml_acos(x)
+
+#define pow(x,y)     ml_pow(x,y)
+
+#ifdef LINUX
+/* stubs for float version of math functions */
+#define cosf(x)      ml_cos(x)
+#define sinf(x)      ml_sin(x)
+#define atan2f(x,y)  ml_atan2(x,y)
+#define sqrtf(x)     ml_sqrt(x)
+#endif
+
+
+
+#endif // MLMATH
+
+#ifndef M_PI
+#define M_PI 3.14159265358979
+#endif
+
+#ifndef ABS
+#define ABS(x) (((x)>=0)?(x):-(x))
+#endif
+
+#ifndef MIN
+#define MIN(x,y) (((x)<(y))?(x):(y))
+#endif
+
+#ifndef MAX
+#define MAX(x,y) (((x)>(y))?(x):(y))
+#endif
+
+/*---------------------------*/
+#endif /* !_ML_MATH_H_ */
diff --git a/libsensors/mlsdk/platform/include/mlos.h b/libsensors/mlsdk/platform/include/mlos.h
new file mode 100644
index 0000000..97c8c88
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/mlos.h
@@ -0,0 +1,109 @@
+/*
+ $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 _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+#include <stdint.h>
+typedef uintptr_t HANDLE; // TODO: shouldn't this be pthread_mutex_t* ???
+#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);
+
+	inv_error_t inv_destroy_mutex(HANDLE handle);
+
+	void inv_sleep(int mSecs);
+	unsigned long inv_get_tick_count(void);
+
+	/* Kernel implmentations */
+#define GFP_KERNEL (0x70)
+	static inline void *kmalloc(size_t size,
+				    unsigned int gfp_flags __unused)
+	{
+		return inv_malloc((unsigned int)size);
+	}
+	static inline void *kzalloc(size_t size, unsigned int gfp_flags __unused)
+	{
+		void *tmp = inv_malloc((unsigned int)size);
+		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/mlsdk/platform/include/mlsl.h b/libsensors/mlsdk/platform/include/mlsl.h
new file mode 100644
index 0000000..535d117
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/mlsl.h
@@ -0,0 +1,290 @@
+/*
+ $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 __MLSL_H__
+#define __MLSL_H__
+
+/**
+ *  @defgroup   MLSL
+ *  @brief      Motion Library - Serial Layer.
+ *              The Motion Library System Layer provides the Motion Library
+ *              with the communication interface to the hardware.
+ *
+ *              The communication interface is assumed to support serial
+ *              transfers in burst of variable length up to
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
+ *              Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
+ *              be subdivided in smaller transfers of length <=
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
+ *              overcome any host processor transfer size limitation down to
+ *              1 B, the minimum.
+ *              An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
+ *              performance and efficiency while requiring higher resource usage
+ *              (mostly buffering). A smaller value will increase overhead and
+ *              decrease efficiency but allows to operate with more resource
+ *              constrained processor and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file.
+ *
+ *  @{
+ *      @file   mlsl.h
+ *      @brief  The Motion Library System Layer.
+ *
+ */
+
+#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
+
+#ifndef __KERNEL__
+/**
+ *  inv_serial_open() - used to open the serial port.
+ *  @port	The COM port specification associated with the device in use.
+ *  @sl_handle	a pointer to the file handle to the serial device to be open
+ *		for the communication.
+ *	This port is used to send and receive data to the device.
+ *
+ *	This function is called by inv_serial_start().
+ *	Unlike previous MPL Software releases, explicitly calling
+ *	inv_serial_start() is mandatory to instantiate the communication
+ *	with the device.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_open(char const *port, void **sl_handle);
+
+/**
+ *  inv_serial_close() - used to close the serial port.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *
+ *	This port is used to send and receive data to the device.
+ *
+ *	This function is called by inv_serial_stop().
+ *	Unlike previous MPL Software releases, explicitly calling
+ *	inv_serial_stop() is mandatory to properly shut-down the
+ *	communication with the device.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t 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);
+#endif
+
+/**
+ *  inv_serial_single_write() - used to write a single byte of data.
+ *  @sl_handle		pointer to the serial device used for the communication.
+ *  @slave_addr		I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @data		Single byte of data to write.
+ *
+ *	It is called by the MPL to write a single byte of data to the MPU.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_single_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned char data);
+
+/**
+ *  inv_serial_write() - used to write multiple bytes of data to registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ *  inv_serial_read() - used to read multiple bytes of data from registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to read.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_read(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_read_mem() - used to read multiple bytes of data from the memory.
+ *	    This should be sent by I2C or SPI.
+ *
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to read from.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_mem() - used to write multiple bytes of data to the memory.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to write to.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ *  inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+#ifndef __KERNEL__
+/**
+ *  inv_serial_read_cfg() - used to get the configuration data.
+ *  @cfg	Pointer to the configuration data.
+ *  @len	Length of the configuration data.
+ *
+ *		Is called by the MPL to get the configuration data
+ *		used by the motion library.
+ *		This data would typically be saved in non-volatile memory.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
+
+/**
+ *  inv_serial_write_cfg() - used to save the configuration data.
+ *  @cfg	Pointer to the configuration data.
+ *  @len	Length of the configuration data.
+ *
+ *		Is called by the MPL to save the configuration data used by the
+ *		motion library.
+ *		This data would typically be saved in non-volatile memory.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
+
+/**
+ *  inv_serial_read_cal() - used to get the calibration data.
+ *  @cfg	Pointer to the calibration data.
+ *  @len	Length of the calibration data.
+ *
+ *		It is called by the MPL to get the calibration data used by the
+ *		motion library.
+ *		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_read_cal(unsigned char *cal, unsigned int len);
+
+/**
+ *  inv_serial_write_cal() - used to save the calibration data.
+ *
+ *  @cfg	Pointer to the calibration data.
+ *  @len	Length of the calibration data.
+ *
+ *	    It is called by the MPL to save the calibration data used by the
+ *	    motion library.
+ *	    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);
+
+/**
+ *  inv_serial_get_cal_length() - Get the calibration length from the storage.
+ *  @len	lenght to be returned
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_serial_get_cal_length(unsigned int *len);
+#endif
+#ifdef __cplusplus
+}
+#endif
+/**
+ * @}
+ */
+#endif				/* __MLSL_H__ */
diff --git a/libsensors/mlsdk/platform/include/mltypes.h b/libsensors/mlsdk/platform/include/mltypes.h
new file mode 100644
index 0000000..90a126b
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/mltypes.h
@@ -0,0 +1,265 @@
+/*
+ $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/libsensors/mlsdk/platform/include/mpu3050.h b/libsensors/mlsdk/platform/include/mpu3050.h
new file mode 100644
index 0000000..bd0ac32
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/mpu3050.h
@@ -0,0 +1,251 @@
+/*
+ $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
+
+#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/libsensors/mlsdk/platform/include/stdint_invensense.h b/libsensors/mlsdk/platform/include/stdint_invensense.h
new file mode 100644
index 0000000..d238813
--- /dev/null
+++ b/libsensors/mlsdk/platform/include/stdint_invensense.h
@@ -0,0 +1,51 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/kernel/mpuirq.h b/libsensors/mlsdk/platform/linux/kernel/mpuirq.h
new file mode 100644
index 0000000..352170b
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/kernel/mpuirq.h
@@ -0,0 +1,41 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/kernel/slaveirq.h b/libsensors/mlsdk/platform/linux/kernel/slaveirq.h
new file mode 100644
index 0000000..beb352b
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/kernel/slaveirq.h
@@ -0,0 +1,35 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/kernel/timerirq.h b/libsensors/mlsdk/platform/linux/kernel/timerirq.h
new file mode 100644
index 0000000..dc3eea2
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/kernel/timerirq.h
@@ -0,0 +1,29 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/log_linux.c b/libsensors/mlsdk/platform/linux/log_linux.c
new file mode 100644
index 0000000..7f7de0e
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/log_linux.c
@@ -0,0 +1,114 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/log_printf_linux.c b/libsensors/mlsdk/platform/linux/log_printf_linux.c
new file mode 100644
index 0000000..e6499f3
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/log_printf_linux.c
@@ -0,0 +1,43 @@
+/*
+ $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/libsensors/mlsdk/platform/linux/mlos_linux.c b/libsensors/mlsdk/platform/linux/mlos_linux.c
new file mode 100644
index 0000000..8704f4b
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/mlos_linux.c
@@ -0,0 +1,206 @@
+/*
+ $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: 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/mlsdk/platform/linux/mlsl_linux_mpu.c b/libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c
new file mode 100644
index 0000000..00d5973
--- /dev/null
+++ b/libsensors/mlsdk/platform/linux/mlsl_linux_mpu.c
@@ -0,0 +1,489 @@
+/*
+ $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"
+#include "mpu3050.h"
+
+#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;
+    unsigned 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;
+    unsigned 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;
+    unsigned 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;
+    unsigned 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*)(uintptr_t) open(port, O_RDWR);
+    if((intptr_t)*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)(uintptr_t)sl_handle);
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_reset(void *sl_handle __unused)
+{
+    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 __unused,
+                         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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        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 __unused,
+                        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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        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 __unused,
+                            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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        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 __unused,
+                           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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        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 __unused,
+                             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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        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 __unused,
+                            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)(uintptr_t)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) - strlen(data_buff) - 1));
+        }
+        MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
+                 MPUREG_FIFO_R_W, length, data_buff);
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @}
+ */
+
+
diff --git a/libsensors/sensor_params.h b/libsensors/sensor_params.h
new file mode 100644
index 0000000..056e587
--- /dev/null
+++ b/libsensors/sensor_params.h
@@ -0,0 +1,46 @@
+/*
+ * 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_YAS530
+#define COMPASS_YAS530_RANGE        (8001.0f)
+#define COMPASS_YAS530_RESOLUTION   (0.012f)
+#define COMPASS_YAS530_POWER        (4.0f)
+/*******************************************/
+//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)
+/******************************************/
+//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)
+
+#endif
+
diff --git a/libsensors/sensors.h b/libsensors/sensors.h
index 18517f9..786feab 100644
--- a/libsensors/sensors.h
+++ b/libsensors/sensors.h
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef ANDROID_SAMSUNG_SENSORS_H
-#define ANDROID_SAMSUNG_SENSORS_H
+#ifndef TUNA_SENSORS_H
+#define TUNA_SENSORS_H
 
 #include <stdint.h>
 #include <errno.h>
@@ -33,6 +33,15 @@ __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)
+
 #define ID_SAMSUNG_BASE (0x1000)
 #define ID_L  (ID_SAMSUNG_BASE)
 #define ID_P  (ID_L + 1)
@@ -44,4 +53,4 @@ __BEGIN_DECLS
 
 __END_DECLS
 
-#endif  /* ANDROID_SAMSUNG_SENSORS_H */
+#endif  /* TUNA_SENSORS_H */