Revert "Remove files for unsupported devices."

This reverts commit f5f584ee173faef40f226c6e0e8580a2ecbe079b.

Change-Id: I4e1b41922b5ccaac2314dac7f43df5740e2e9361

149 files changed, 37957 insertions, 1 deletions

diff --git a/60xx/Android.mk b/60xx/Android.mk
new file mode 100644
index 0000000..427bbb4
--- /dev/null
+++ b/60xx/Android.mk
@@ -0,0 +1,8 @@
+# Can't have both manta and non-manta libsensors.
+ifneq ($(filter manta, $(TARGET_DEVICE)),)
+# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta.
+include $(call all-named-subdir-makefiles,libsensors_iio)
+else
+# libsensors expects an non-IIO MPU3050.
+include $(call all-named-subdir-makefiles,mlsdk libsensors)
+endif
diff --git a/60xx/libsensors/Android.mk b/60xx/libsensors/Android.mk
new file mode 100644
index 0000000..326e096
--- /dev/null
+++ b/60xx/libsensors/Android.mk
@@ -0,0 +1,47 @@
+# 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.
+# Modified 2011 by InvenSense, Inc
+
+
+LOCAL_PATH := $(call my-dir)
+
+ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
+
+# InvenSense fragment of the HAL
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := libinvensense_hal
+
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall
+LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1
+
+LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp 
+
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include/linux
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/linux
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mllite
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mldmp
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/aichi
+LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/akmd
+
+LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
+LOCAL_CPPFLAGS+=-DLINUX=1
+LOCAL_LDFLAGS:=-rdynamic
+
+include $(BUILD_SHARED_LIBRARY)
+
+endif
diff --git a/60xx/libsensors/MPLSensor.cpp b/60xx/libsensors/MPLSensor.cpp
new file mode 100644
index 0000000..4676d0e
--- /dev/null
+++ b/60xx/libsensors/MPLSensor.cpp
@@ -0,0 +1,1304 @@
+/*
+ * 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, 0, 0, 0, 0, { } },
+      { "MPL Accelerometer", "Invensense", 1,
+         SENSORS_ACCELERATION_HANDLE,
+         SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
+      { "MPL Magnetic Field", "Invensense", 1,
+         SENSORS_MAGNETIC_FIELD_HANDLE,
+         SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
+      { "MPL Orientation", "Invensense", 1,
+         SENSORS_ORIENTATION_HANDLE,
+         SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } },
+      { "MPL Rotation Vector", "Invensense", 1,
+         SENSORS_ROTATION_VECTOR_HANDLE,
+         SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
+      { "MPL Linear Acceleration", "Invensense", 1,
+         SENSORS_LINEAR_ACCEL_HANDLE,
+         SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
+      { "MPL Gravity", "Invensense", 1,
+         SENSORS_GRAVITY_HANDLE,
+         SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
+};
+
+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), mPollTime(-1),
+            mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
+            mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
+            mUseTimerirq(false), mSampleCount(0),
+            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;
+        mPollTime = -1;
+        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;
+    mSampleCount++;
+}
+
+//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;
+}
+
+/* return the current time in nanoseconds */
+int64_t MPLSensor::now_ns(void)
+{
+    //FUNC_LOG;
+    struct timespec ts;
+
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+}
+
+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
+{
+    int hdl = (uintptr_t) inv_get_serial_handle();
+    return hdl;
+}
+
+int MPLSensor::getPollTime()
+{
+    return mPollTime;
+}
+
+bool MPLSensor::hasPendingEvents() const
+{
+    //if we are using the polling workaround, force the main loop to check for data every time
+    return (mPollTime != -1);
+}
+
+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                          */
+    unsigned short accelId = inv_get_accel_id();
+    if(accelId == 0) {
+       ALOGE("Can not get accel id");
+    }
+    fillAccel(accelId, list);
+
+    /* fill in compass values                        */
+    unsigned short compassId = inv_get_compass_id();
+    if(compassId == 0) {
+       ALOGE("Can not get compass id");
+    }
+    fillCompass(compassId, list);
+
+    /* fill in gyro values                           */
+    fillGyro(MPU_NAME, list);
+
+    if(mNineAxisEnabled)
+    {
+        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;
+}
+
+void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list)
+{
+    switch (accel) {
+    case ACCEL_ID_LIS331:
+        list[Accelerometer].maxRange = ACCEL_LIS331_RANGE;
+        list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION;
+        list[Accelerometer].power = ACCEL_LIS331_POWER;
+        break;
+
+    case ACCEL_ID_LIS3DH:
+        list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE;
+        list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION;
+        list[Accelerometer].power = ACCEL_LIS3DH_POWER;
+        break;
+
+    case ACCEL_ID_KXSD9:
+        list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE;
+        list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION;
+        list[Accelerometer].power = ACCEL_KXSD9_POWER;
+        break;
+
+    case ACCEL_ID_KXTF9:
+        list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE;
+        list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION;
+        list[Accelerometer].power = ACCEL_KXTF9_POWER;
+        break;
+
+    case ACCEL_ID_BMA150:
+        list[Accelerometer].maxRange = ACCEL_BMA150_RANGE;
+        list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION;
+        list[Accelerometer].power = ACCEL_BMA150_POWER;
+        break;
+
+    case ACCEL_ID_BMA222:
+        list[Accelerometer].maxRange = ACCEL_BMA222_RANGE;
+        list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION;
+        list[Accelerometer].power = ACCEL_BMA222_POWER;
+        break;
+
+    case ACCEL_ID_BMA250:
+        list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+        list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+        list[Accelerometer].power = ACCEL_BMA250_POWER;
+        break;
+
+    case ACCEL_ID_ADXL34X:
+        list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE;
+        list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION;
+        list[Accelerometer].power = ACCEL_ADXL34X_POWER;
+        break;
+
+    case ACCEL_ID_MMA8450:
+        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
+        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
+        list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
+        break;
+
+    case ACCEL_ID_MMA845X:
+        list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE;
+        list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION;
+        list[Accelerometer].power = ACCEL_MMA845X_POWER;
+        break;
+
+    case ACCEL_ID_MPU6050:
+        list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
+        list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
+        list[Accelerometer].power = ACCEL_MPU6050_POWER;
+        break;
+    default:
+        ALOGE("unknown accel id -- accel params will be wrong.");
+        break;
+    }
+}
+
+void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list)
+{
+    switch (compass) {
+    case COMPASS_ID_AK8975:
+        list[MagneticField].maxRange = COMPASS_AKM8975_RANGE;
+        list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION;
+        list[MagneticField].power = COMPASS_AKM8975_POWER;
+        break;
+    case COMPASS_ID_AMI30X:
+        list[MagneticField].maxRange = COMPASS_AMI30X_RANGE;
+        list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION;
+        list[MagneticField].power = COMPASS_AMI30X_POWER;
+        break;
+    case COMPASS_ID_AMI306:
+        list[MagneticField].maxRange = COMPASS_AMI306_RANGE;
+        list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
+        list[MagneticField].power = COMPASS_AMI306_POWER;
+        break;
+    case COMPASS_ID_YAS529:
+        list[MagneticField].maxRange = COMPASS_YAS529_RANGE;
+        list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
+        list[MagneticField].power = COMPASS_AMI306_POWER;
+        break;
+    case COMPASS_ID_YAS530:
+        list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
+        list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
+        list[MagneticField].power = COMPASS_YAS530_POWER;
+        break;
+    case COMPASS_ID_HMC5883:
+        list[MagneticField].maxRange = COMPASS_HMC5883_RANGE;
+        list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION;
+        list[MagneticField].power = COMPASS_HMC5883_POWER;
+        break;
+    case COMPASS_ID_MMC314X:
+        list[MagneticField].maxRange = COMPASS_MMC314X_RANGE;
+        list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION;
+        list[MagneticField].power = COMPASS_MMC314X_POWER;
+        break;
+    case COMPASS_ID_HSCDTD002B:
+        list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE;
+        list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION;
+        list[MagneticField].power = COMPASS_HSCDTD002B_POWER;
+        break;
+    case COMPASS_ID_HSCDTD004A:
+        list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE;
+        list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION;
+        list[MagneticField].power = COMPASS_HSCDTD004A_POWER;
+        break;
+    default:
+        ALOGE("unknown compass id -- compass parameters will be wrong");
+    }
+}
+
+void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
+{
+    if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) {
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+    } else {
+        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
+        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6050_POWER;
+    }
+    return;
+}
+
+
+/* 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/60xx/libsensors/MPLSensor.h b/60xx/libsensors/MPLSensor.h
new file mode 100644
index 0000000..f580732
--- /dev/null
+++ b/60xx/libsensors/MPLSensor.h
@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2011 Invensense, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/*************Removed the gesture related info for Google check in : Meenakshi Ramamoorthi: May 31st *********/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include "sensors.h"
+#include "SensorBase.h"
+
+/*****************************************************************************/
+/** MPLSensor implementation which fits into the HAL example for crespo provided
+ * * by Google.
+ * * WARNING: there may only be one instance of MPLSensor, ever.
+ */
+
+class MPLSensor: public SensorBase
+{
+    typedef void (MPLSensor::*hfunc_t)(sensors_event_t*, uint32_t*, int);
+
+public:
+    MPLSensor();
+    virtual ~MPLSensor();
+
+    enum
+    {
+        Gyro=0,
+        Accelerometer,
+        MagneticField,
+        Orientation,
+        RotationVector,
+        LinearAccel,
+        Gravity,
+        numSensors
+    };
+
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    virtual int readEvents(sensors_event_t *data, int count);
+    virtual int getFd() const;
+    virtual int getAccelFd() const;
+    virtual int getTimerFd() const;
+    virtual int getPowerFd() const;
+    virtual int getPollTime();
+    virtual bool hasPendingEvents() const;
+    virtual void handlePowerEvent();
+    virtual void sleepEvent();
+    virtual void wakeEvent();
+    int populateSensorList(struct sensor_t *list, 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 mPollTime;
+    int mCurFifoRate; //current fifo rate
+    bool mHaveGoodMpuCal; //flag indicating that the cal file can be written
+    bool mHaveGoodCompassCal;
+    bool mUseTimerIrqAccel;
+    bool mUsetimerIrqCompass;
+    bool mUseTimerirq;
+    struct pollfd mPollFds[4];
+    int mSampleCount;
+    pthread_mutex_t mMplMutex;
+    int64_t now_ns();
+    int64_t select_ns(unsigned long long time_set[]);    
+
+    enum FILEHANDLES
+    {
+        MPUIRQ_FD, ACCELIRQ_FD, COMPASSIRQ_FD, TIMERIRQ_FD,
+    };
+
+private:
+
+    int update_delay();
+    int accel_fd;
+    int timer_fd;
+
+    uint32_t mEnabled;
+    uint32_t mPendingMask;
+    sensors_event_t mPendingEvents[numSensors];
+    uint64_t mDelays[numSensors];
+    hfunc_t mHandlers[numSensors];
+    bool mForceSleep;
+    long int mOldEnabledMask;
+    android::KeyedVector<int, int> mIrqFds;
+
+    /* added for dynamic get sensor list              */
+    bool mNineAxisEnabled;
+    void fillAccel(unsigned char accel, struct sensor_t *list);
+    void fillCompass(unsigned char compass, struct sensor_t *list);
+    void fillGyro(const char* gyro, struct sensor_t *list);
+    void fillRV(struct sensor_t *list);
+    void fillOrientation(struct sensor_t *list);
+    void fillGravity(struct sensor_t *list);
+    void fillLinearAccel(struct sensor_t *list);
+};
+
+void setCallbackObject(MPLSensor*);
+
+/*****************************************************************************/
+
+#endif  // ANDROID_MPL_SENSOR_H
diff --git a/60xx/libsensors/SensorBase.cpp b/60xx/libsensors/SensorBase.cpp
new file mode 100644
index 0000000..9cc1ee8
--- /dev/null
+++ b/60xx/libsensors/SensorBase.cpp
@@ -0,0 +1,129 @@
+/*
+ * 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, int64_t ns) {
+    return 0;
+}
+
+bool SensorBase::hasPendingEvents() const {
+    return false;
+}
+
+int64_t SensorBase::getTimestamp() {
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return int64_t(t.tv_sec)*1000000000LL + t.tv_nsec;
+}
+
+int SensorBase::openInput(const char* inputName) {
+    int fd = -1;
+    const char *dirname = "/dev/input";
+    char devname[PATH_MAX];
+    char *filename;
+    DIR *dir;
+    struct dirent *de;
+    dir = opendir(dirname);
+    if(dir == NULL)
+        return -1;
+    strcpy(devname, dirname);
+    filename = devname + strlen(devname);
+    *filename++ = '/';
+    while((de = readdir(dir))) {
+        if(de->d_name[0] == '.' &&
+                (de->d_name[1] == '\0' ||
+                        (de->d_name[1] == '.' && de->d_name[2] == '\0')))
+            continue;
+        strcpy(filename, de->d_name);
+        fd = open(devname, O_RDONLY);
+        if (fd>=0) {
+            char name[80];
+            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
+                name[0] = '\0';
+            }
+            if (!strcmp(name, inputName)) {
+                strcpy(input_name, filename);
+                break;
+            } else {
+                close(fd);
+                fd = -1;
+            }
+        }
+    }
+    closedir(dir);
+    ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName);
+    return fd;
+}
diff --git a/60xx/libsensors/SensorBase.h b/60xx/libsensors/SensorBase.h
new file mode 100644
index 0000000..bb4d055
--- /dev/null
+++ b/60xx/libsensors/SensorBase.h
@@ -0,0 +1,65 @@
+/*
+ * 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);
+    static int64_t getTimestamp();
+
+
+    static int64_t timevalToNano(timeval const& t) {
+        return t.tv_sec*1000000000LL + t.tv_usec*1000;
+    }
+
+    int open_device();
+    int close_device();
+
+public:
+            SensorBase(
+                    const char* dev_name,
+                    const char* data_name);
+
+    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/60xx/libsensors/sensor_params.h b/60xx/libsensors/sensor_params.h
new file mode 100644
index 0000000..4925ac4
--- /dev/null
+++ b/60xx/libsensors/sensor_params.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2011 Invensense, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef INV_SENSOR_PARAMS_H
+#define INV_SENSOR_PARAMS_H
+
+/* Physical parameters of the sensors supported by Invensense MPL */
+#define SENSORS_ROTATION_VECTOR_HANDLE  (ID_RV)
+#define SENSORS_LINEAR_ACCEL_HANDLE     (ID_LA)
+#define SENSORS_GRAVITY_HANDLE          (ID_GR)
+#define SENSORS_GYROSCOPE_HANDLE        (ID_GY)
+#define SENSORS_ACCELERATION_HANDLE     (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE   (ID_M)
+#define SENSORS_ORIENTATION_HANDLE      (ID_O)
+/******************************************/
+//COMPASS_ID_AKM
+#define COMPASS_AKM8975_RANGE       (9830.0f)
+#define COMPASS_AKM8975_RESOLUTION  (0.285f)
+#define COMPASS_AKM8975_POWER       (10.0f)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE        (5461.0f)
+#define COMPASS_AMI30X_RESOLUTION   (0.9f)
+#define COMPASS_AMI30X_POWER        (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE        (5461.0f)
+#define COMPASS_AMI306_RESOLUTION   (0.9f)
+#define COMPASS_AMI306_POWER        (0.15f)
+//COMPASS_ID_YAS529
+#define COMPASS_YAS529_RANGE        (19660.0f)
+#define COMPASS_YAS529_RESOLUTION   (0.012f)
+#define COMPASS_YAS529_POWER        (4.0f)
+//COMPASS_ID_YAS530
+#define COMPASS_YAS530_RANGE        (8001.0f)
+#define COMPASS_YAS530_RESOLUTION   (0.012f)
+#define COMPASS_YAS530_POWER        (4.0f)
+//COMPASS_ID_HMC5883
+#define COMPASS_HMC5883_RANGE       (10673.0f)
+#define COMPASS_HMC5883_RESOLUTION  (10.0f)
+#define COMPASS_HMC5883_POWER       (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE     (10240.0f)
+#define COMPASS_LSM303DLH_RESOLUTION    (1.0f)
+#define COMPASS_LSM303DLH_POWER     (1.0f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE     (10240.0f)
+#define COMPASS_LSM303DLM_RESOLUTION    (1.0f)
+#define COMPASS_LSM303DLM_POWER     (1.0f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE       (400.0f)
+#define COMPASS_MMC314X_RESOLUTION  (2.0f)
+#define COMPASS_MMC314X_POWER       (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE    (9830.0f)
+#define COMPASS_HSCDTD002B_RESOLUTION   (1.0f)
+#define COMPASS_HSCDTD002B_POWER    (1.0f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE    (9830.0f)
+#define COMPASS_HSCDTD004A_RESOLUTION   (1.0f)
+#define COMPASS_HSCDTD004A_POWER    (1.0f)
+/*******************************************/
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE      (2.480f*GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION     (.001f*GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER      (1.0f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE       (2.480f*GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION  (0.001f*GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER       (1.0f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE      (2.480f*GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION     (0.001f*GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER      (1.0f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE       (2.5006f*GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION      (0.001f*GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER       (1.0f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE       (1.0f*GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION      (0.033f*GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER       (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE      (2.0f*GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION     (0.004f*GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER      (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE      (2.0f*GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION     (0.001f*GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER      (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE      (2.0f*GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION     (0.00391f*GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER      (0.139f)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE     (2.0f*GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION    (0.001f*GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER     (1.0f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE     (2.0f*GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION    (0.001f*GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER     (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE     (2.0f*GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION    (0.001f*GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER     (1.0f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE     (2.0f*GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION    (0.004f*GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER     (0.0f)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG (3.14159f/180.0f)
+#define GYRO_MPU3050_RANGE      (2000.0f*RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION     (32.8f*RAD_P_DEG)
+#define GYRO_MPU3050_POWER      (6.1f)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE      (2000.0f*RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION     (16.4f*RAD_P_DEG)
+#define GYRO_MPU6050_POWER      (5.5f)
+
+#endif
+
diff --git a/60xx/libsensors/sensors.h b/60xx/libsensors/sensors.h
new file mode 100644
index 0000000..5c56da0
--- /dev/null
+++ b/60xx/libsensors/sensors.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2008 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ANDROID_SENSORS_H
+#define ANDROID_SENSORS_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+
+__BEGIN_DECLS
+
+/*****************************************************************************/
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+
+#define ID_MPL_BASE (0)
+#define ID_GY (ID_MPL_BASE)
+#define ID_A  (ID_GY + 1)
+#define ID_M  (ID_A + 1)
+#define ID_O  (ID_M + 1)
+#define ID_RV (ID_O + 1) 
+#define ID_LA (ID_RV + 1)
+#define ID_GR (ID_LA + 1)
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+__END_DECLS
+
+#endif  // ANDROID_SENSORS_H
diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
new file mode 100644
index 0000000..fd8e472
--- /dev/null
+++ b/60xx/libsensors_iio/Android.mk
@@ -0,0 +1,118 @@
+# 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.
+# Modified 2011 by InvenSense, Inc
+
+LOCAL_PATH := $(call my-dir)
+
+# InvenSense fragment of the HAL
+include $(CLEAR_VARS)
+
+LOCAL_MODULE := libinvensense_hal
+
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall
+# Comment out for ICS. Affects Android LOG macros.
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+
+ifeq ($(ENG_BUILD),1)
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS53x
+endif
+ifeq ($(COMPILE_COMPASS_AK8975),1)
+LOCAL_CFLAGS += -DCOMPASS_AK8975
+endif
+ifeq ($(COMPILE_COMPASS_AMI306),1)
+LOCAL_CFLAGS += -DCOMPASS_AMI306
+endif
+else # release builds, default
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+
+LOCAL_SRC_FILES := SensorBase.cpp
+LOCAL_SRC_FILES += MPLSensor.cpp
+LOCAL_SRC_FILES += MPLSupport.cpp
+LOCAL_SRC_FILES += InputEventReader.cpp
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+
+LOCAL_C_INCLUDES += $(LOCAL_PATH)
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_SHARED_LIBRARIES := liblog
+LOCAL_SHARED_LIBRARIES += libcutils
+LOCAL_SHARED_LIBRARIES += libutils
+LOCAL_SHARED_LIBRARIES += libdl
+LOCAL_SHARED_LIBRARIES += libmllite
+
+# Additions for SysPed
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_CPPFLAGS += -DLINUX=1
+# Experimental
+ifeq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true)
+LOCAL_CPPFLAGS += -DLIBMLLITE_FROM_SOURCE
+endif
+
+include $(BUILD_SHARED_LIBRARY)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmplmpu
+LOCAL_SRC_FILES := libmplmpu.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+LOCAL_STRIP_MODULE := true
+include $(BUILD_PREBUILT)
+
+# Experimental
+ifneq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true)
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_SRC_FILES := libmllite.so
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_SUFFIX := .so
+LOCAL_MODULE_CLASS := SHARED_LIBRARIES
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
+LOCAL_STRIP_MODULE := true
+include $(BUILD_PREBUILT)
+else
+include $(CLEAR_VARS)
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+LOCAL_CFLAGS += -DLINUX=1
+LOCAL_MODULE := libmllite
+LOCAL_SRC_FILES := $(call all-c-files-under, software/core/mllite)
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+LOCAL_MODULE_OWNER := invensense
+LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
+LOCAL_SHARED_LIBRARIES := liblog
+include $(BUILD_SHARED_LIBRARY)
+endif
diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
new file mode 100644
index 0000000..be8c912
--- /dev/null
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.9150.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+#include "local_log_def.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+
+#if defined COMPASS_YAS53x
+#   pragma message "Invensense compass cal with YAS53x IIO on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_YAS530"
+#elif defined COMPASS_AK8975
+#   pragma message "Invensense compass cal with AK8975 on primary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_AK8975"
+#elif defined INVENSENSE_COMPASS_CAL
+#   pragma message "Invensense compass cal with compass IIO on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_COMPASS"
+#else
+#   pragma message "third party compass cal HAL"
+#   define USE_MPL_COMPASS_HAL          (0)
+// TODO: change to vendor's name
+#   define COMPASS_NAME                 "AKM8975"
+#endif
+
+
+/*****************************************************************************/
+
+CompassSensor::CompassSensor() 
+                  : SensorBase(NULL, NULL),
+                    compass_fd(-1),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8)
+{
+    VFUNC_LOG;
+
+    if(inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(COMPASS_NAME, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_orient, getTimestamp());
+    FILE *fptr;
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:compass mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mCompassOrientation[0] = om[0];
+        mCompassOrientation[1] = om[1];
+        mCompassOrientation[2] = om[2];
+        mCompassOrientation[3] = om[3];
+        mCompassOrientation[4] = om[4];
+        mCompassOrientation[5] = om[5];
+        mCompassOrientation[6] = om[6];
+        mCompassOrientation[7] = om[7];
+        mCompassOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read compass mounting matrix");
+    }
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+}
+
+int CompassSensor::getFd() const
+{
+    VHANDLER_LOG;
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable; 
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t /*handle*/, int en) 
+{
+    VFUNC_LOG;
+
+    mEnable = en;
+    int res;
+
+    res = write_sysfs_int(compassSysFs.compass_enable, en);
+    LOGE_IF(res < 0, "HAL:enable compass failed");
+
+    if (en) {
+        res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
+        res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
+        res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
+    }
+
+    return res;
+}
+
+int CompassSensor::setDelay(int32_t /*handle*/, int64_t ns) 
+{
+    VFUNC_LOG;
+    int tempFd;
+    int res;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 
+            1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
+    mDelay = ns;
+    if (ns == 0)
+        return -1;
+    tempFd = open(compassSysFs.compass_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / ns);
+    if(res < 0) {
+        LOGE("HAL:Compass update delay error");
+    }
+    return res;
+}
+
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t /*handle*/)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/* use for Invensense compass calibration */
+#define COMPASS_EVENT_DEBUG (0)
+void CompassSensor::processCompassEvent(const input_event *event)
+{
+    VHANDLER_LOG;
+
+    switch (event->code) {
+    case EVENT_TYPE_ICOMPASS_X:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
+        mCachedCompassData[0] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Y:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
+        mCachedCompassData[1] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Z:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
+        mCachedCompassData[2] = event->value;
+        break;
+    }
+    
+    mCompassTimestamp = 
+        (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
+}
+
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+    memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
+}
+
+long CompassSensor::getSensitivity()
+{
+    VFUNC_LOG;
+
+    long sensitivity;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+            compassSysFs.compass_scale, getTimestamp());
+    inv_read_data(compassSysFs.compass_scale, &sensitivity);
+    return sensitivity;
+}
+
+/**
+    @brief         This function is called by sensors_mpl.cpp
+                   to read sensor data from the driver.
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+ */
+int CompassSensor::readSample(long *data, int64_t *timestamp)
+{
+    VHANDLER_LOG;
+
+    int done = 0;
+
+    ssize_t n = mCompassInputReader.fill(compass_fd);
+    if (n < 0) {
+        LOGE("HAL:no compass events read");
+        return n;
+    }
+
+    input_event const* event;
+
+    while (done == 0 && mCompassInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_REL) {
+            processCompassEvent(event);
+        } else if (type == EV_SYN) {
+            *timestamp = mCompassTimestamp;
+            memcpy(data, mCachedCompassData, sizeof(mCachedCompassData));
+            done = 1;
+        } else {
+            LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)",
+                 type, event->code);
+        }
+        mCompassInputReader.next();
+    }
+
+    return done;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds. 
+ */
+int64_t CompassSensor::getDelay(int32_t /*handle*/)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = COMPASS_NAME;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            /* since target platform would use AK8963 instead of AK8975,
+               need to adopt AK8963's resolution here */
+            list->resolution = COMPASS_AKM8963_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975") ) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_YAS530")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "INV_AMI306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
+        ALOGE("CompassSensor failed get sysfs path");
+        return -1;
+    }
+
+    inv_get_iio_trigger_path(iio_trigger_path);
+
+#if defined COMPASS_AK8975
+    char tbuf[2];
+    int num;
+    inv_get_input_number(COMPASS_NAME, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+    sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
+    sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
+    sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#endif
+
+#if SYSFS_VERBOSE
+    // test print sysfs paths
+    dptr = (char**)&compassSysFs;
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
new file mode 100644
index 0000000..6e9bf03
--- /dev/null
+++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t* /*data*/, int /*count*/) { return 0; }
+
+    int readSample(long *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); }
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *compass_enable;
+       char *compass_x_fifo_enable;
+       char *compass_y_fifo_enable;
+       char *compass_z_fifo_enable;
+       char *compass_rate;
+       char *compass_scale;
+       char *compass_orient;
+    } compassSysFs;
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int compass_fd;
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int64_t mDelay;
+    int mEnable;
+    char *pathP;
+
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/60xx/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp
new file mode 100644
index 0000000..fc48892
--- /dev/null
+++ b/60xx/libsensors_iio/InputEventReader.cpp
@@ -0,0 +1,106 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#define LOG_NDEBUG 0

+

+#include <stdint.h>

+#include <errno.h>

+#include <string.h>

+#include <unistd.h>

+#include <poll.h>

+

+#include <sys/cdefs.h>

+#include <sys/types.h>

+

+#include <linux/input.h>

+

+#include <cutils/log.h>

+

+#include "InputEventReader.h"

+#include "local_log_def.h"

+

+/*****************************************************************************/

+

+struct input_event;

+

+InputEventCircularReader::InputEventCircularReader(size_t numEvents)

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

+      mBufferEnd(mBuffer + numEvents),

+      mHead(mBuffer),

+      mCurr(mBuffer),

+      mFreeSpace(numEvents)

+{

+}

+

+InputEventCircularReader::~InputEventCircularReader()

+{

+    delete [] mBuffer;

+}

+

+#define INPUT_EVENT_DEBUG (0)

+ssize_t InputEventCircularReader::fill(int fd)

+{

+    size_t numEventsRead = 0;

+    LOGV_IF(INPUT_EVENT_DEBUG, 

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

+    if (mFreeSpace) {

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

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

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

+            //     nread, sizeof(input_event));

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

+            // we got a partial event!!

+            if (INPUT_EVENT_DEBUG) {

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

+                        __PRETTY_FUNCTION__);

+                LOGV_IF(nread % sizeof(input_event), 

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

+                        __PRETTY_FUNCTION__);

+            }

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

+        }

+

+        numEventsRead = nread / sizeof(input_event);

+        if (numEventsRead) {

+            mHead += numEventsRead;

+            mFreeSpace -= numEventsRead;

+            if (mHead > mBufferEnd) {

+                size_t s = mHead - mBufferEnd;

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

+                mHead = mBuffer + s;

+            }

+        }

+    }

+

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

+    return numEventsRead;

+}

+

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

+{

+    *events = mCurr;

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

+    return available ? 1 : 0;

+}

+

+void InputEventCircularReader::next()

+{

+    mCurr++;

+    mFreeSpace++;

+    if (mCurr >= mBufferEnd) {

+        mCurr = mBuffer;

+    }

+}

diff --git a/60xx/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h
new file mode 100644
index 0000000..5c752db
--- /dev/null
+++ b/60xx/libsensors_iio/InputEventReader.h
@@ -0,0 +1,49 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_INPUT_EVENT_READER_H
+#define ANDROID_INPUT_EVENT_READER_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include "SensorBase.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+class InputEventCircularReader
+{
+    struct input_event* const mBuffer;
+    struct input_event* const mBufferEnd;
+    struct input_event* mHead;
+    struct input_event* mCurr;
+    ssize_t mFreeSpace;
+
+public:
+    InputEventCircularReader(size_t numEvents);
+    ~InputEventCircularReader();
+    ssize_t fill(int fd);
+    ssize_t readEvent(input_event const** events);
+    void next();
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_INPUT_EVENT_READER_H
diff --git a/60xx/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt
new file mode 100644
index 0000000..930f931
--- /dev/null
+++ b/60xx/libsensors_iio/License.txt
@@ -0,0 +1,217 @@
+SOFTWARE LICENSE AGREEMENT
+
+Unless you and InvenSense Corporation ("InvenSense") execute a separate written
+software license agreement governing use of the accompanying software, this
+software is licensed to you under the terms of this Software License
+Agreement ("Agreement").
+
+ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR
+ACCEPTANCE OF THIS AGREEMENT.
+
+1.  DEFINITIONS.
+
+1.1.    "InvenSense Product" means any of the proprietary integrated circuit
+product(s) sold by InvenSense with which the Software was designed to be used,
+or their successors.
+
+1.2.    "Licensee" means you or if you are accepting on behalf of an entity
+then the entity and its affiliates exercising rights under, and complying
+with all of the terms of this Agreement.
+
+1.3.    "Software" shall mean that software made available by InvenSense to
+Licensee in binary code form with this Agreement.
+
+2.  LICENSE GRANT; OWNERSHIP
+
+2.1.    License Grants.  Subject to the terms and conditions of this Agreement,
+InvenSense hereby grants to Licensee a non-exclusive, non-transferable,
+royalty-free license (i) to use and integrate the Software in conjunction
+with any other software; and (ii) to reproduce and distribute the Software
+complete, unmodified and only for use with a InvenSense Product.
+
+2.2.    Restriction on Modification.  If and to the extent that the Software is
+designed to be compliant with any published communications standard
+(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards),
+Licensee may not make any modifications to the Software that would cause the
+Software or the accompanying InvenSense Products to be incompatible with such
+standard.
+
+2.3.    Restriction on Distribution.  Licensee shall only distribute the
+Software (a) under the terms of this Agreement and a copy of this Agreement
+accompanies such distribution, and (b) agrees to defend and indemnify
+InvenSense and its licensors from and against any damages, costs, liabilities,
+settlement amounts and/or expenses (including attorneys' fees) incurred in
+connection with any claim, lawsuit or action by any third party that arises
+or results from the use or distribution of any and all Software by the
+Licensee except as contemplated herein.
+
+2.4.    Proprietary Notices.  Licensee shall not remove, efface or obscure any
+copyright or trademark notices from the Software.  Licensee shall include
+reproductions of the InvenSense copyright notice with each copy of the
+Software, except where such Software is embedded in a manner not readily
+accessible to the end user.  Licensee acknowledges that any symbols,
+trademarks, tradenames, and service marks adopted by InvenSense to identify the
+Software belong to InvenSense and that Licensee shall have no rights therein.
+
+2.5.    Ownership.  InvenSense shall retain all right, title and interest,
+including all intellectual property rights, in and to the Software.  Licensee
+hereby covenants that it will not assert any claim that the Software created
+by or for InvenSense infringe any intellectual property right owned or
+controlled by Licensee.
+
+2.6.    No Other Rights Granted; Restrictions.  Apart from the license rights
+expressly set forth in this Agreement, InvenSense does not grant and Licensee
+does not receive any ownership right, title or interest nor any security
+interest or other interest in any intellectual property rights relating to
+the Software, nor in any copy of any part of the foregoing.  No license is
+granted to Licensee in any human readable code of the Software (source code).
+Licensee shall not (i) use, license, sell or otherwise distribute the
+Software except as provided in this Agreement, (ii) attempt to reverse
+engineer, decompile or disassemble any portion of the Software; or (iii) use
+the Software or other material in violation of any applicable law or
+regulation, including but not limited to any regulatory agency, such as FCC,
+rules.
+
+3.  NO WARRANTY OR SUPPORT
+
+3.1.    No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND
+LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE,
+COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE.  INVENSENSE SPECIFICALLY
+DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC
+PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR
+DOCUMENTATION FOR THE SOFTWARE.  WITHOUT LIMITATION OF THE ABOVE, INVENSENSE
+GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT
+INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS
+THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR
+RELIABILITY.
+
+3.2.    No Support.  Nothing in this agreement shall obligate InvenSense to
+provide any support for the Software. InvenSense may, but shall be under no
+obligation to, correct any defects in the Software and/or provide updates to
+licensees of the Software.  Licensee shall make reasonable efforts to
+promptly report to InvenSense any defects it finds in the Software, as an aid
+to creating improved revisions of the Software.
+
+3.3.    Dangerous Applications.  The Software is not designed, intended, or
+certified for use in components of systems intended for the operation of
+weapons, weapons systems, nuclear installations, means of mass
+transportation, aviation, life-support computers or equipment (including
+resuscitation equipment and surgical implants), pollution control, hazardous
+substances management, or for any other dangerous application in which the
+failure of the Software could create a situation where personal injury or
+death may occur.  Licensee understands that use of the Software in such
+applications is fully at the risk of Licensee.
+
+4.  TERM AND TERMINATION
+
+4.1.    Termination.  This Agreement will automatically terminate if Licensee
+fails to comply with any of the terms and conditions hereof. In such event,
+Licensee must destroy all copies of the Software and all of its component
+parts.
+
+4.2.    Effect Of Termination.  Upon any termination of this Agreement, the
+rights and licenses granted to Licensee under this Agreement shall
+immediately terminate.
+
+4.3.    Survival.  The rights and obligations under this Agreement which by
+their nature should survive termination will remain in effect after
+expiration or termination of this Agreement.
+
+5.  CONFIDENTIALITY
+
+5.1.    Obligations.  Licensee acknowledges and agrees that any documentation
+relating to the Software, and any other information (if such other
+information is identified as confidential or should be recognized as
+confidential under the circumstances) provided to Licensee by InvenSense
+hereunder (collectively, "Confidential Information") constitute the
+confidential and proprietary information of InvenSense, and that Licensee's
+protection thereof is an essential condition to Licensee's use and possession
+of the Software.  Licensee shall retain all Confidential Information in
+strict confidence and not disclose it to any third party or use it in any way
+except under a written agreement with terms and conditions at least as
+protective as the terms of this Section.  Licensee will exercise at least the
+same amount of diligence in preserving the secrecy of the Confidential
+Information as it uses in preserving the secrecy of its own most valuable
+confidential information, but in no event less than reasonable diligence.
+Information shall not be considered Confidential Information if and to the
+extent that it: (i) was in the public domain at the time it was disclosed or
+has entered the public domain through no fault of Licensee; (ii) was known to
+Licensee, without restriction, at the time of disclosure as proven by the
+files of Licensee in existence at the time of disclosure; or (iii) becomes
+known to Licensee, without restriction, from a source other than InvenSense
+without breach of this Agreement by Licensee and otherwise not in violation
+of InvenSense's rights.
+
+5.2.    Return of Confidential Information.  Notwithstanding the foregoing, all
+documents and other tangible objects containing or representing InvenSense
+Confidential Information and all copies thereof which are in the possession
+of Licensee shall be and remain the property of InvenSense, and shall be
+promptly returned to InvenSense upon written request by InvenSense or upon
+termination of this Agreement.
+
+6.  LIMITATION OF LIABILITY
+TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF
+INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL,
+SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR
+OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS
+OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT
+(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR
+SOFTWARE UNDER THIS AGREEMENT.  THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING
+ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY.
+
+7.  MISCELLANEOUS
+
+7.1.    Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS
+SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND
+REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE
+OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS.
+WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE
+TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED.
+
+7.2 Assignment. This Agreement shall be binding upon and inure to the
+benefit of the parties and their respective successors and assigns, provided,
+however that Licensee may not assign this Agreement or any rights or
+obligation hereunder, directly or indirectly, by operation of law or
+otherwise, without the prior written consent of InvenSense, and any such
+attempted assignment shall be void.  Notwithstanding the foregoing, Licensee
+may assign this Agreement to a successor to all or substantially all of its
+business or assets to which this Agreement relates that is not a competitor
+of InvenSense.
+
+7.3.    Governing Law; Venue.  This Agreement shall be governed by the laws of
+California without regard to any conflict-of-laws rules, and the United
+Nations Convention on Contracts for the International Sale of Goods is hereby
+excluded.  The sole jurisdiction and venue for actions related to the subject
+matter hereof shall be the state and federal courts located in the County of
+Orange, California, and both parties hereby consent to such jurisdiction and
+venue.
+
+7.4.    Severability.  All terms and provisions of this Agreement shall, if
+possible, be construed in a manner which makes them valid, but in the event
+any term or provision of this Agreement is found by a court of competent
+jurisdiction to be illegal or unenforceable, the validity or enforceability
+of the remainder of this Agreement shall not be affected if the illegal or
+unenforceable provision does not materially affect the intent of this
+Agreement.  If the illegal or unenforceable provision materially affects the
+intent of the parties to this Agreement, this Agreement shall become
+terminated.
+
+7.5.    Equitable Relief.  Licensee hereby acknowledges that its breach of this
+Agreement would cause irreparable harm and significant injury to InvenSense
+that may be difficult to ascertain and that a remedy at law would be
+inadequate.  Accordingly, Licensee agrees that InvenSense shall have the right
+to seek and obtain immediate injunctive relief to enforce obligations under
+the Agreement in addition to any other rights and remedies it may have.
+
+7.6.    Waiver.  The waiver of, or failure to enforce, any breach or default
+hereunder shall not constitute the waiver of any other or subsequent breach
+or default.
+
+7.7.    Entire Agreement.  This Agreement sets forth the entire Agreement
+between the parties and supersedes any and all prior proposals, agreements
+and representations between them, whether written or oral concerning the
+Software.  This Agreement may be changed only by mutual agreement of the
+parties in writing.
+
diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp
new file mode 100644
index 0000000..031ae6e
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSensor.cpp
@@ -0,0 +1,2764 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <float.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <sys/syscall.h>
+#include <dlfcn.h>
+#include <pthread.h>
+#include <cutils/log.h>
+#include <utils/KeyedVector.h>
+#include <utils/String8.h>
+#include <string.h>
+#include <linux/input.h>
+#include <utils/Atomic.h>
+
+#include "MPLSensor.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "local_log_def.h"
+
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+
+// #define TESTING
+
+#ifdef THIRD_PARTY_ACCEL
+#   warning "Third party accel"
+#   define USE_THIRD_PARTY_ACCEL        (1)
+#else
+#   define USE_THIRD_PARTY_ACCEL        (0)
+#endif
+
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+/******************************************************************************/
+/*  MPL interface misc.                                                       */
+/******************************************************************************/
+static int hertz_request = 200;
+
+#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
+#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
+
+#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
+#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
+#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
+#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
+
+/* convert ns to hardware units */
+#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
+#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
+#define HW_COMPASS_RATE_NS              (rate_request)                // to ns
+
+/* convert Hz to hardware units */
+#define HW_GYRO_RATE_HZ                 (hertz_request)
+#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
+#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
+
+#define RATE_200HZ                      5000000LL
+#define RATE_15HZ                       66667000LL
+#define RATE_5HZ                        200000000LL
+
+static struct sensor_t sSensorList[] =
+{
+    {"MPL Gyroscope", "Invensense", 1,
+     SENSORS_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Raw Gyroscope", "Invensense", 1,
+     SENSORS_RAW_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Accelerometer", "Invensense", 1,
+     SENSORS_ACCELERATION_HANDLE,
+     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Magnetic Field", "Invensense", 1,
+     SENSORS_MAGNETIC_FIELD_HANDLE,
+     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Orientation", "Invensense", 1,
+     SENSORS_ORIENTATION_HANDLE,
+     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Rotation Vector", "Invensense", 1,
+     SENSORS_ROTATION_VECTOR_HANDLE,
+     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Linear Acceleration", "Invensense", 1,
+     SENSORS_LINEAR_ACCEL_HANDLE,
+     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+    {"MPL Gravity", "Invensense", 1,
+     SENSORS_GRAVITY_HANDLE,
+     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    {"MPL Screen Orientation", "Invensense ", 1,
+     SENSORS_SCREEN_ORIENTATION_HANDLE,
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}},
+#endif
+};
+
+MPLSensor *MPLSensor::gMPLSensor = NULL;
+
+extern "C" {
+void procData_cb_wrapper()
+{
+    if(MPLSensor::gMPLSensor) {
+        MPLSensor::gMPLSensor->cbProcData();
+    }
+}
+
+void setCallbackObject(MPLSensor* gbpt)
+{
+    MPLSensor::gMPLSensor = gbpt;
+}
+
+MPLSensor* getCallbackObject() {
+    return MPLSensor::gMPLSensor;
+}
+
+} // end of extern C
+
+#ifdef INV_PLAYBACK_DBG
+static FILE *logfile = NULL;
+#endif
+
+pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
+
+/*******************************************************************************
+ * MPLSensor class implementation
+ ******************************************************************************/
+
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
+                       : SensorBase(NULL, NULL),
+                         mNewData(0),
+                         mMasterSensorMask(INV_ALL_SENSORS),
+                         mLocalSensorMask(0),
+                         mPollTime(-1),
+                         mHaveGoodMpuCal(0),
+                         mGyroAccuracy(0),
+                         mAccelAccuracy(0),
+                         mCompassAccuracy(0),
+                         mSampleCount(0),
+                         dmp_orient_fd(-1),
+                         mDmpOrientationEnabled(0),
+                         mEnabled(0),
+                         mOldEnabledMask(0),
+                         mAccelInputReader(4),
+                         mGyroInputReader(32),
+                         mTempScale(0),
+                         mTempOffset(0),
+                         mTempCurrentTime(0),
+                         mAccelScale(2),
+                         mPendingMask(0),
+                         mSensorMask(0),
+                         mFeatureActiveMask(0) {
+    VFUNC_LOG;
+
+    inv_error_t rv;
+    int fd;
+    char *ver_str;
+
+    mCompassSensor = compass;
+
+    LOGV_IF(EXTRA_VERBOSE,
+            "HAL:MPLSensor constructor : numSensors = %d", numSensors);
+
+    pthread_mutex_init(&mMplMutex, NULL);
+    pthread_mutex_init(&mHALMutex, NULL);
+    memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
+    memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
+
+#ifdef INV_PLAYBACK_DBG
+    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
+    logfile = fopen("/data/playback.bin", "wb");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    /* setup sysfs paths */
+    inv_init_sysfs_attributes();
+
+    /* get chip name */
+    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
+        LOGE("HAL:ERR- Failed to get chip ID\n");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
+    }
+
+    enable_iio_sysfs();
+
+    /* turn on power state */
+    onPower(1);
+
+    /* reset driver master enable */
+    masterEnable(0);
+
+    if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+        /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+        loadDMP();
+    }
+
+    /* open temperature fd for temp comp */
+    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
+    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
+    if (gyro_temperature_fd == -1) {
+        LOGE("HAL:could not open temperature node");
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:temperature_fd opened: %s", mpu.temperature);
+    }
+
+    /* read accel FSR to calcuate accel scale later */
+    if (!USE_THIRD_PARTY_ACCEL) {
+        char buf[3];
+        int count = 0;
+        LOGV_IF(SYSFS_VERBOSE,
+                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
+
+        fd = open(mpu.accel_fsr, O_RDONLY);
+        if(fd < 0) {
+            LOGE("HAL:Error opening accel FSR");
+        } else {
+           memset(buf, 0, sizeof(buf));
+           count = read_attribute_sensor(fd, buf, sizeof(buf));
+           if(count < 1) {
+               LOGE("HAL:Error reading accel FSR");
+           } else {
+               count = sscanf(buf, "%d", &mAccelScale);
+               if(count)
+                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
+           }
+           close(fd);
+        }
+    }
+
+    /* initialize sensor data */
+    memset(mPendingEvents, 0, sizeof(mPendingEvents));
+
+    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
+    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[RawGyro].version = sizeof(sensors_event_t);
+    mPendingEvents[RawGyro].sensor = ID_RG;
+    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
+
+    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+    mPendingEvents[Accelerometer].sensor = ID_A;
+    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+
+    /* Invensense compass calibration */
+    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
+    mPendingEvents[MagneticField].sensor = ID_M;
+    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
+    mPendingEvents[MagneticField].magnetic.status =
+        SENSOR_STATUS_ACCURACY_HIGH;
+
+    mPendingEvents[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[RawGyro] = &MPLSensor::rawGyroHandler;
+    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
+    mHandlers[MagneticField] = &MPLSensor::compassHandler;
+    mHandlers[Orientation] = &MPLSensor::orienHandler;
+
+    for (int i = 0; i < numSensors; i++) {
+        mDelays[i] = 0;
+    }
+
+    (void)inv_get_version(&ver_str);
+    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
+
+    /* setup MPL */
+    inv_constructor_init();
+
+    /* load calibration file from /data/inv_cal_data.bin */
+    rv = inv_load_calibration();
+    if(rv == INV_SUCCESS)
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
+    else
+        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
+
+    /* Takes external Accel Calibration Load Method */
+    if( m_pt2AccelCalLoadFunc != NULL)
+    {
+        long accel_offset[3];
+        long tmp_offset[3];
+        int result = m_pt2AccelCalLoadFunc(accel_offset);
+        if(result)
+            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
+        else
+        {
+            LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+            inv_set_accel_bias(accel_offset, mAccelAccuracy);
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+        }
+    }
+    /* End of Accel Calibration Load Method */
+
+    inv_set_device_properties();
+
+    /* disable driver master enable the first sensor goes on */
+    masterEnable(0);
+    enableGyro(0);
+    enableAccel(0);
+    enableCompass(0);
+
+    if (isLowPowerQuatEnabled()) {
+        enableLPQuaternion(0);
+    }
+
+    onPower(0);
+
+    if (isDmpDisplayOrientationOn()) {
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
+    }
+
+}
+
+void MPLSensor::enable_iio_sysfs()
+{
+    VFUNC_LOG;
+
+    char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+
+    /* ignore failures */
+    write_sysfs_int(mpu.in_timestamp_en, 1);
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)",
+            mpu.trigger_name, getTimestamp());
+    tempFp = fopen(mpu.trigger_name, "r");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open trigger name");
+    } else {
+        if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not read trigger name");
+        }
+        fclose(tempFp);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
+            iio_trigger_name, mpu.current_trigger, getTimestamp());
+    tempFp = fopen(mpu.current_trigger, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open current trigger");
+    } else {
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
+        }
+    }
+
+    write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
+
+    if (inv_get_iio_device_node(iio_device_node) < 0) {
+        LOGE("HAL:could retrive the iio device node");
+    }
+    iio_fd = open(iio_device_node, O_RDONLY);
+    if (iio_fd < 0) {
+        LOGE("HAL:could not open iio device node");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
+    }
+}
+
+int MPLSensor::inv_constructor_init()
+{
+    VFUNC_LOG;
+
+    inv_error_t result = inv_init_mpl();
+    if (result) {
+        LOGE("HAL:inv_init_mpl() failed");
+        return result;
+    }
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOGE("HAL:inv_start_mpl() failed");
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+int MPLSensor::inv_constructor_default_enable()
+{
+    VFUNC_LOG;
+
+    inv_error_t result;
+
+    result = inv_enable_quaternion();
+    if (result) {
+        LOGE("HAL:Cannot enable quaternion\n");
+        return result;
+    }
+
+    result = inv_enable_in_use_auto_calibration();
+    if (result) {
+        return result;
+    }
+
+    // result = inv_enable_motion_no_motion();
+    result = inv_enable_fast_nomot();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_gyro_tc();
+    if (result) {
+        return result;
+    }
+
+    result = inv_enable_hal_outputs();
+    if (result) {
+        return result;
+    }
+
+    if (!mCompassSensor->providesCalibration()) {
+        /* Invensense compass calibration */
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
+        result = inv_enable_vector_compass_cal();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        } else {
+            mFeatureActiveMask |= INV_COMPASS_CAL;
+        }
+
+        // specify MPL's trust weight, used by compass algorithms
+        inv_vector_compass_cal_sensitivity(3);
+
+        result = inv_enable_compass_bias_w_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_enable_heading_from_gyro();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_enable_magnetic_disturbance();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        // 9x sensor fusion enables Compass fit
+        mFeatureActiveMask |= INV_COMPASS_FIT;
+    }
+
+    result = inv_enable_no_gyro_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    result = inv_enable_quat_accuracy_monitor();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/* TODO: create function pointers to calculate scale */
+void MPLSensor::inv_set_device_properties()
+{
+    VFUNC_LOG;
+
+    unsigned short orient;
+
+    inv_get_sensors_orientation();
+
+    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
+    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
+
+    /* gyro setup */
+    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
+    inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
+
+    /* accel setup */
+    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
+    /* use for third party accel input subsystem driver
+    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    */
+    inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
+
+    /* compass setup */
+    signed char orientMtx[9];
+    mCompassSensor->getOrientationMatrix(orientMtx);
+    orient =
+        inv_orientation_matrix_to_scalar(orientMtx);
+    long sensitivity;
+    sensitivity = mCompassSensor->getSensitivity();
+    inv_set_compass_orientation_and_scale(orient, sensitivity);
+}
+
+void MPLSensor::loadDMP()
+{
+    int res, fd;
+    FILE *fptr;
+
+    if (isMpu3050()) {
+        //DMP support only for MPU6xxx/9xxx currently
+        return;
+    }
+
+    /* load DMP firmware */
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
+    fd = open(mpu.firmware_loaded, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:could not open dmp state");
+        return;
+    }
+    if(inv_read_dmp_state(fd)) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
+        return;
+    }
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+    fptr = fopen(mpu.dmp_firmware, "w");
+    if(!fptr) {
+        LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
+        return;
+    }
+    res = inv_load_dmp(fptr);
+    if(res < 0) {
+        LOGE("HAL:load DMP failed");
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
+    }
+    fclose(fptr);
+}
+
+void MPLSensor::inv_get_sensors_orientation()
+{
+    FILE *fptr;
+
+    // get gyro orientation
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
+    fptr = fopen(mpu.gyro_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:gyro mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mGyroOrientation[0] = om[0];
+        mGyroOrientation[1] = om[1];
+        mGyroOrientation[2] = om[2];
+        mGyroOrientation[3] = om[3];
+        mGyroOrientation[4] = om[4];
+        mGyroOrientation[5] = om[5];
+        mGyroOrientation[6] = om[6];
+        mGyroOrientation[7] = om[7];
+        mGyroOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read gyro mounting matrix");
+    }
+
+    // get accel orientation
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
+    fptr = fopen(mpu.accel_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:accel mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mAccelOrientation[0] = om[0];
+        mAccelOrientation[1] = om[1];
+        mAccelOrientation[2] = om[2];
+        mAccelOrientation[3] = om[3];
+        mAccelOrientation[4] = om[4];
+        mAccelOrientation[5] = om[5];
+        mAccelOrientation[6] = om[6];
+        mAccelOrientation[7] = om[7];
+        mAccelOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read accel mounting matrix");
+    }
+}
+
+MPLSensor::~MPLSensor()
+{
+    VFUNC_LOG;
+
+    mCompassSensor = NULL;
+
+    /* Close open fds */
+    if (iio_fd > 0)
+        close(iio_fd);
+    if( accel_fd > 0 )
+        close(accel_fd );
+    if (gyro_temperature_fd > 0)
+        close(gyro_temperature_fd);
+    if (sysfs_names_ptr)
+        free(sysfs_names_ptr);
+
+    if (isDmpDisplayOrientationOn()) {
+        closeDmpOrientFd();
+    }
+
+    /* Turn off Gyro master enable          */
+    /* A workaround until driver handles it */
+    /* TODO: Turn off and close all sensors */
+    if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
+        LOGE("HAL:could not disable gyro master enable");
+    }
+
+#ifdef INV_PLAYBACK_DBG
+    inv_turn_off_data_logging();
+    fclose(logfile);
+#endif
+}
+
+#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
+#define A_ENABLED  ((1 << ID_A)  & enabled_sensors)
+#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
+#define O_ENABLED  ((1 << ID_O)  & enabled_sensors)
+#define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
+#define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
+#define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
+
+/* TODO: this step is optional, remove?  */
+int MPLSensor::setGyroInitialState()
+{
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
+    int fd = open(mpu.gyro_fifo_rate, O_RDWR);
+    res = errno;
+    if(fd < 0) {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.gyro_fifo_rate, strerror(res), res);
+        return res;
+    }
+    res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
+    if(res < 0) {
+        LOGE("HAL:write_attribute_sensor : error writing %s with %d",
+             mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
+        return res;
+    }
+
+    // Setting LPF is deprecated
+
+    return 0;
+}
+
+/* this applies to BMA250 Input Subsystem Driver only */
+int MPLSensor::setAccelInitialState()
+{
+    VFUNC_LOG;
+
+    struct input_absinfo absinfo_x;
+    struct input_absinfo absinfo_y;
+    struct input_absinfo absinfo_z;
+    float value;
+    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
+        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
+        value = absinfo_x.value;
+        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
+        value = absinfo_y.value;
+        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
+        value = absinfo_z.value;
+        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
+    }
+    return 0;
+}
+
+int MPLSensor::onPower(int en)
+{
+    VFUNC_LOG;
+
+    int res;
+
+    int curr_power_state;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.power_state, getTimestamp());
+    res = read_sysfs_int(mpu.power_state, &curr_power_state);
+    if (res < 0) {
+        LOGE("HAL:Error reading power state");
+        // will set power_state anyway
+        curr_power_state = -1;
+    }
+    if (en != curr_power_state) {
+        if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
+                LOGE("HAL:Couldn't write power state");
+        }
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:Power state already enable/disable curr=%d new=%d",
+                curr_power_state, en);
+    }
+    return res;
+}
+
+int MPLSensor::onDMP(int en)
+{
+    VFUNC_LOG;
+
+    int res = -1;
+    int status;
+
+    //Sequence to enable DMP
+    //1. Turn On power if not already on
+    //2. Load DMP image if not already loaded
+    //3. Either Gyro or Accel must be enabled/configured before next step
+    //4. Enable DMP
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.firmware_loaded, getTimestamp());
+    res = read_sysfs_int(mpu.firmware_loaded, &status);
+    if (res < 0){
+        LOGE("HAL:ERR can't get firmware_loaded status");
+        return res;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
+
+    if (status) {
+        //Write only if curr DMP state <> request
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+                mpu.dmp_on, getTimestamp());
+        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
+            LOGE("HAL:ERR can't read DMP state");
+        } else if (status != en) {
+            res = write_sysfs_int(mpu.dmp_on, en);
+            //Enable DMP interrupt
+            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
+                LOGE("HAL:ERR can't en/dis DMP interrupt");
+            }
+        } else {
+            res = 0;    //DMP already set as requested
+        }
+    } else {
+        LOGE("HAL:ERR No DMP image");
+    }
+    return res;
+}
+
+int MPLSensor::checkLPQuaternion(void)
+{
+    VFUNC_LOG;
+
+    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
+}
+
+int MPLSensor::enableLPQuaternion(int en)
+{
+    VFUNC_LOG;
+
+    if (!en) {
+        enableQuaternionData(0);
+        onDMP(0);
+        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
+    } else {
+        if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
+            LOGE("HAL:ERR can't enable LP Quaternion");
+        } else {
+            mFeatureActiveMask |= INV_DMP_QUATERNION;
+            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enableQuaternionData(int en)
+{
+    int res = 0;
+    VFUNC_LOG;
+
+    // Enable DMP quaternion
+    res = write_sysfs_int(mpu.quaternion_on, en);
+
+    if (!en) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
+    } else {
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
+    }
+    write_sysfs_int(mpu.in_quat_r_en, en);
+    write_sysfs_int(mpu.in_quat_x_en, en);
+    write_sysfs_int(mpu.in_quat_y_en, en);
+    write_sysfs_int(mpu.in_quat_z_en, en);
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
+
+    if (!en) {
+        inv_quaternion_sensor_was_turned_off();
+    }
+
+    return res;
+}
+
+int MPLSensor::enableTap(int /*en*/)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::enableFlick(int /*en*/)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::enablePedometer(int /*en*/)
+{
+    VFUNC_LOG;
+
+    return 0;
+}
+
+int MPLSensor::masterEnable(int en)
+{
+    VFUNC_LOG;
+    return write_sysfs_int(mpu.chip_enable, en);
+}
+
+int MPLSensor::enableGyro(int en)
+{
+    VFUNC_LOG;
+
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res;
+
+    /* need to also turn on/off the master enable */
+    res = write_sysfs_int(mpu.gyro_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
+        inv_gyro_was_turned_off();
+    } else {
+        write_sysfs_int(mpu.gyro_x_fifo_enable, en);
+        write_sysfs_int(mpu.gyro_y_fifo_enable, en);
+        res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
+    }
+
+    return res;
+}
+
+int MPLSensor::enableAccel(int en)
+{
+    VFUNC_LOG;
+
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res;
+
+    /* need to also turn on/off the master enable */
+    res = write_sysfs_int(mpu.accel_enable, en);
+
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
+        inv_accel_was_turned_off();
+    } else {
+        write_sysfs_int(mpu.accel_x_fifo_enable, en);
+        write_sysfs_int(mpu.accel_y_fifo_enable, en);
+        res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
+    }
+
+    return res;
+}
+
+int MPLSensor::enableCompass(int en)
+{
+    VFUNC_LOG;
+
+    int res = mCompassSensor->enable(ID_M, en);
+    if (!en) {
+        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
+        inv_compass_was_turned_off();
+    }
+    return res;
+}
+
+void MPLSensor::computeLocalSensorMask(int enabled_sensors)
+{
+    VFUNC_LOG;
+
+    do {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
+            mLocalSensorMask = ALL_MPL_SENSORS_NP;
+            break;
+        }
+
+        if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
+            /* Invensense compass cal */
+            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
+            mLocalSensorMask = 0;
+            break;
+        }
+
+        if (GY_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "G ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "G DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
+        }
+
+        if (A_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "A ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "A DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
+        }
+
+        /* Invensense compass calibration */
+        if (M_ENABLED) {
+            LOGV_IF(ENG_VERBOSE, "M ENABLED");
+            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
+        } else {
+            LOGV_IF(ENG_VERBOSE, "M DISABLED");
+            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
+        }
+    } while (0);
+}
+
+int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
+    return (this->*enabler)(en);
+}
+
+int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) {
+    VFUNC_LOG;
+
+    inv_error_t res = -1;
+    bool store_cal = false;
+    bool ext_compass_changed = false;
+
+    // Sequence to enable or disable a sensor
+    // 1. enable Power state
+    // 2. reset master enable (=0)
+    // 3. enable or disable a sensor
+    // 4. set master enable (=1)
+
+    pthread_mutex_lock(&GlobalHalMutex);
+
+    uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
+            | (1 << MagneticField);
+    uint32_t all_integrated_changeables = all_changeables;
+
+    if (!mCompassSensor->isIntegrated()) {
+        ext_compass_changed = changed & (1 << MagneticField);
+        all_integrated_changeables = all_changeables & ~(1 << MagneticField);
+    }
+
+    if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
+        /* ensure power state is on */
+        onPower(1);
+
+        /* reset master enable */
+        res = masterEnable(0);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
+
+    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+        res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if (changed & (1 << Accelerometer)) {
+        res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if (changed & (1 << MagneticField)) {
+        /* Invensense compass calibration */
+        res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
+        if(res < 0) {
+            goto unlock_res;
+        }
+    }
+
+    if ( isLowPowerQuatEnabled() ) {
+        // Enable LP Quat
+        if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                (1 << LinearAccel) | (1 << Gravity)))) {
+            if (!(changed & all_integrated_changeables)) {
+                /* ensure power state is on */
+                onPower(1);
+                /* reset master enable */
+                res = masterEnable(0);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            if (!checkLPQuaternion()) {
+                enableLPQuaternion(1);
+            } else {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+            }
+        } else if (checkLPQuaternion()) {
+            enableLPQuaternion(0);
+        }
+    }
+
+    if (changed & all_integrated_changeables) {
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            if ( isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
+                // disable DMP event interrupt only (w/ data interrupt)
+                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't disable DMP event interrupt");
+                    goto unlock_res;
+                }
+            }
+
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+
+                res = enableAccel(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+
+            res = masterEnable(1);
+            if(res < 0) {
+                goto unlock_res;
+            }
+        } else { // all sensors idle -> reduce power
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+                // enable DMP event interrupt only (no data interrupt)
+                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                }
+                res = enableAccel(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    goto unlock_res;
+                }
+                res = masterEnable(1);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            else {
+                res = onPower(0);
+                if(res < 0) {
+                    goto unlock_res;
+                }
+            }
+            store_cal = true;
+        }
+    } else if (ext_compass_changed &&
+            !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
+            store_cal = true;
+    }
+
+    if (store_cal || ((changed & all_changeables) != all_changeables)) {
+        storeCalibration();
+    }
+
+unlock_res:
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+/* Store calibration file */
+void MPLSensor::storeCalibration()
+{
+    if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
+       int res = inv_store_calibration();
+       if (res) {
+           LOGE("HAL:Cannot store calibration on file");
+       } else {
+           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
+       }
+    }
+}
+
+void MPLSensor::cbProcData()
+{
+    mNewData = 1;
+    mSampleCount++;
+    LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
+}
+
+/*  these handlers transform mpl data into one of the Android sensor types */
+int MPLSensor::gyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::rawGyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::accelHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_accelerometer(
+        s->acceleration.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
+            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
+            s->timestamp, update);
+    mAccelAccuracy = status;
+    return update;
+}
+
+int MPLSensor::compassHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_magnetic_field(
+        s->magnetic.v, &s->magnetic.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
+            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
+    mCompassAccuracy = s->magnetic.status;
+    return update;
+}
+
+int MPLSensor::rvHandler(sensors_event_t* s)
+{
+    // rotation vector does not have an accuracy or status
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
+            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::laHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_linear_acceleration(
+            s->gyro.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::gravHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int8_t status;
+    int update;
+    update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::orienHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_orientation(
+            s->orientation.v, &s->orientation.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
+            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
+    return update;
+}
+
+int MPLSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1, err = 0;
+
+    switch (handle) {
+    case ID_SO:
+        sname = "Screen Orientation";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
+            (mDmpOrientationEnabled? "en": "dis"),
+            (en? "en" : "dis"));
+        enableDmpOrientation(en && isDmpDisplayOrientationOn());
+        /* TODO: stop manually testing/using 0 and 1 instead of
+         * false and true, but just use 0 and non-0.
+         * This allows  passing 0 and non-0 ints around instead of
+         * having to convert to 1 and test against 1.
+         */
+        mDmpOrientationEnabled = en;
+        return 0;
+    case ID_A:
+        what = Accelerometer;
+        sname = "Accelerometer";
+        break;
+    case ID_M:
+        what = MagneticField;
+        sname = "MagneticField";
+        break;
+    case ID_O:
+        what = Orientation;
+        sname = "Orientation";
+        break;
+    case ID_GY:
+        what = Gyro;
+        sname = "Gyro";
+        break;
+    case ID_RG:
+        what = RawGyro;
+        sname = "RawGyro";
+        break;
+    case ID_GR:
+        what = Gravity;
+        sname = "Gravity";
+        break;
+    case ID_RV:
+        what = RotationVector;
+        sname = "RotationVector";
+        break;
+    case ID_LA:
+        what = LinearAccel;
+        sname = "LinearAccel";
+        break;
+    default: //this takes care of all the gestures
+        what = handle;
+        sname = "Others";
+        break;
+    }
+
+    if (uint32_t(what) >= numSensors)
+        return -EINVAL;
+
+    int newState = en ? 1 : 0;
+    unsigned long sen_mask;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
+            ((mEnabled & (1 << what)) ? "en" : "dis"),
+            ((uint32_t(newState) << what) ? "en" : "dis"));
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:%s sensor state change what=%d", sname.string(), what);
+
+    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
+        short flags = newState;
+        uint32_t lastEnabled = mEnabled, changed = 0;
+
+        mEnabled &= ~(1 << what);
+        mEnabled |= (uint32_t(flags) << what);
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
+        computeLocalSensorMask(mEnabled);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
+        sen_mask = mLocalSensorMask & mMasterSensorMask;
+        mSensorMask = sen_mask;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
+
+        switch (what) {
+            case Gyro:
+            case RawGyro:
+            case Accelerometer:
+            case MagneticField:
+                if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))) &&
+                        ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
+                    changed |= (1 << what);
+                }
+                break;
+
+            case Orientation:
+            case RotationVector:
+            case LinearAccel:
+            case Gravity:
+                if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity)))) ||
+                        (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                        (1 << LinearAccel) | (1 << Gravity))))) {
+                    for (int i = Gyro; i <= MagneticField; i++) {
+                        if (!(mEnabled & (1 << i))) {
+                            changed |= (1 << i);
+                        }
+                    }
+                }
+                break;
+        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
+        enableSensors(sen_mask, flags, changed);
+    }
+
+#ifdef INV_PLAYBACK_DBG
+    /* apparently the logging needs to be go through this sequence
+       to properly flush the log file */
+    inv_turn_off_data_logging();
+    fclose(logfile);
+    logfile = fopen("/data/playback.bin", "ab");
+    if (logfile)
+        inv_turn_on_data_logging(logfile);
+#endif
+
+    return err;
+}
+
+int MPLSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+
+    android::String8 sname;
+    int what = -1;
+
+    switch (handle) {
+        case ID_SO:
+            return update_delay();
+        case ID_A:
+            what = Accelerometer;
+            sname = "Accelerometer";
+            break;
+        case ID_M:
+            what = MagneticField;
+            sname = "MagneticField";
+            break;
+        case ID_O:
+            what = Orientation;
+            sname = "Orientation";
+            break;
+        case ID_GY:
+            what = Gyro;
+            sname = "Gyro";
+            break;
+        case ID_RG:
+            what = RawGyro;
+            sname = "RawGyro";
+            break;
+        case ID_GR:
+            what = Gravity;
+            sname = "Gravity";
+            break;
+        case ID_RV:
+            what = RotationVector;
+            sname = "RotationVector";
+            break;
+        case ID_LA:
+            what = LinearAccel;
+            sname = "LinearAccel";
+            break;
+        default: // this takes care of all the gestures
+            what = handle;
+            sname = "Others";
+            break;
+    }
+
+#if 0
+    // skip the 1st call for enalbing sensors called by ICS/JB sensor service
+    static int counter_delay = 0;
+    if (!(mEnabled & (1 << what))) {
+        counter_delay = 0;
+    } else {
+        if (++counter_delay == 1) {
+            return 0;
+        }
+        else {
+            counter_delay = 0;
+        }
+    }
+#endif
+
+    if (uint32_t(what) >= numSensors)
+        return -EINVAL;
+
+    if (ns < 0)
+        return -EINVAL;
+
+    LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
+
+    // limit all rates to reasonable ones */
+    if (ns < 5000000LL) {
+        ns = 5000000LL;
+    }
+
+    /* store request rate to mDelays array for each sensor */
+    mDelays[what] = ns;
+
+    switch (what) {
+        case Gyro:
+        case RawGyro:
+        case Accelerometer:
+            for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
+                    i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+
+        case MagneticField:
+            if (mCompassSensor->isIntegrated() &&
+                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
+                    ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
+                 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
+                 return 0;
+            }
+            break;
+
+        case Orientation:
+        case RotationVector:
+        case LinearAccel:
+        case Gravity:
+            if (isLowPowerQuatEnabled()) {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
+                break;
+            }
+
+            for (int i = 0; i < numSensors; i++) {
+                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
+                    LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
+                    return 0;
+                }
+            }
+            break;
+    }
+
+    int res = update_delay();
+    return res;
+}
+
+int MPLSensor::update_delay() {
+    VHANDLER_LOG;
+
+    int res = 0;
+    int64_t got;
+
+    pthread_mutex_lock(&GlobalHalMutex);
+    if (mEnabled) {
+        int64_t wanted = 1000000000;
+        int64_t wanted_3rd_party_sensor = 1000000000;
+
+        // Sequence to change sensor's FIFO rate
+        // 1. enable Power state
+        // 2. reset master enable
+        // 3. Update delay
+        // 4. set master enable
+
+        // ensure power on
+        onPower(1);
+
+        // reset master enable
+        masterEnable(0);
+
+        /* search the minimum delay requested across all enabled sensors */
+        for (int i = 0; i < numSensors; i++) {
+            if (mEnabled & (1 << i)) {
+                int64_t ns = mDelays[i];
+                wanted = wanted < ns ? wanted : ns;
+            }
+        }
+
+        // same delay for 3rd party Accel or Compass
+        wanted_3rd_party_sensor = wanted;
+
+        /* mpl rate in us in future maybe different for
+           gyro vs compass vs accel */
+        int rateInus = (int)wanted / 1000LL;
+        int mplGyroRate = rateInus;
+        int mplAccelRate = rateInus;
+        int mplCompassRate = rateInus;
+
+        LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
+             "%llu ns, mpl rate: %d us, (%.2f Hz)",
+             wanted, rateInus, 1000000000.f / wanted);
+
+        /* set rate in MPL */
+        /* compass can only do 100Hz max */
+        inv_set_gyro_sample_rate(mplGyroRate);
+        inv_set_accel_sample_rate(mplAccelRate);
+        inv_set_compass_sample_rate(mplCompassRate);
+#ifdef LIBMLLITE_FROM_SOURCE
+        inv_set_linear_acceleration_sample_rate(rateInus);
+        inv_set_gravity_sample_rate(rateInus);
+#endif
+
+        /* TODO: Test 200Hz */
+        // inv_set_gyro_sample_rate(5000);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
+
+        int enabled_sensors = mEnabled;
+        int tempFd = -1;
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            if (isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
+                bool setDMPrate= 0;
+                // Set LP Quaternion sample rate if enabled
+                if (checkLPQuaternion()) {
+                    if (wanted < RATE_200HZ) {
+                        enableLPQuaternion(0);
+                    } else {
+                        inv_set_quat_sample_rate(rateInus);
+                        setDMPrate= 1;
+                    }
+                }
+
+                if (checkDMPOrientation() || setDMPrate==1) {
+                    getDmpRate(&wanted);
+                }
+            }
+
+            int64_t tempRate = wanted;
+            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
+            //nsToHz
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                    1000000000.f / tempRate, mpu.gyro_fifo_rate,
+                    getTimestamp());
+            tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+            res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
+            if(res < 0) {
+                LOGE("HAL:GYRO update delay error");
+            }
+
+            //nsToHz (BMA250)
+            if(USE_THIRD_PARTY_ACCEL) {
+                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
+                        wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
+                        getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            if (!mCompassSensor->isIntegrated()) {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
+                mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
+
+        } else {
+
+            if (GY_ENABLED) {
+                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    getDmpRate(&wanted);
+                }
+
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
+                tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
+                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
+                LOGE_IF(res < 0, "HAL:GYRO update delay error");
+            }
+
+            if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
+                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[Gyro];
+                }
+                else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[RawGyro];
+
+                } else {
+                    wanted = mDelays[Accelerometer];
+                }
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    getDmpRate(&wanted);
+                }
+
+                /* TODO: use function pointers to calculate delay value specific to vendor */
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+                        1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
+                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
+                if(USE_THIRD_PARTY_ACCEL) {
+                    //BMA250 in ms
+                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                }
+                else {
+                    //MPUxxxx in hz
+                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                }
+                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
+            }
+
+            /* Invensense compass calibration */
+            if (M_ENABLED) {
+                if (!mCompassSensor->isIntegrated()) {
+                    wanted = mDelays[MagneticField];
+                } else {
+                    if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[Gyro];
+                    }
+                    else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[RawGyro];
+                    } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+                        wanted = mDelays[Accelerometer];
+                    } else {
+                        wanted = mDelays[MagneticField];
+                    }
+
+                    if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                        getDmpRate(&wanted);
+                    }
+                }
+
+                mCompassSensor->setDelay(ID_M, wanted);
+                got = mCompassSensor->getDelay(ID_M);
+                inv_set_compass_sample_rate(got / 1000);
+            }
+
+        }
+
+        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            res = masterEnable(1);
+        } else { // all sensors idle -> reduce power
+            res = onPower(0);
+        }
+    }
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+/* For Third Party Accel Input Subsystem Drivers only */
+/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
+int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    ssize_t n = mAccelInputReader.fill(accel_fd);
+    if (n < 0) {
+        LOGE("HAL:missed accel events, exit");
+        return n;
+    }
+
+    int numEventReceived = 0;
+    input_event const* event;
+    int done = 0;
+
+    while (!done && count && mAccelInputReader.readEvent(&event)) {
+        int type = event->type;
+        if (type == EV_ABS) {
+            if (event->code == EVENT_TYPE_ACCEL_X) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[0] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[1] = event->value;
+            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
+                mPendingMask |= 1 << Accelerometer;
+                mCachedAccelData[2] =event-> value;
+            }
+        } else if (type == EV_SYN) {
+            done = 1;
+            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+                inv_build_accel(mCachedAccelData, 0, getTimestamp());
+            }
+        } else {
+            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
+                    type, event->code);
+        }
+        mAccelInputReader.next();
+    }
+
+    return numEventReceived;
+}
+
+/**
+ *  Should be called after reading at least one of gyro
+ *  compass or accel data. (Also okay for handling all of them).
+ *  @returns 0, if successful, error number if not.
+ */
+/* TODO: This should probably be called "int readEvents(...)"
+ *  and readEvents() called "void cacheData(void)".
+ */
+int MPLSensor::executeOnData(sensors_event_t* data, int count)
+{
+    VFUNC_LOG;
+
+    inv_execute_on_data();
+
+    int numEventReceived = 0;
+
+    long msg;
+    msg = inv_get_message_level_0(1);
+    if (msg) {
+        if (msg & INV_MSG_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
+        }
+        if (msg & INV_MSG_NO_MOTION_EVENT) {
+            LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
+            /* after the first no motion, the gyro should be
+               calibrated well */
+            mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
+            /* if gyros are on and we got a no motion, set a flag
+               indicating that the cal file can be written. */
+            mHaveGoodMpuCal = true;
+        }
+    }
+
+    // load up virtual sensors
+    for (int i = 0; i < numSensors; i++) {
+        int update;
+        if (mEnabled & (1 << i)) {
+            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
+            mPendingMask |= (1 << i);
+
+            if (update && (count > 0)) {
+                *data++ = mPendingEvents[i];
+                count--;
+                numEventReceived++;
+            }
+        }
+    }
+
+    return numEventReceived;
+}
+
+// collect data for MPL (but NOT sensor service currently), from driver layer
+/* TODO: FIX! data and count are not used, results is hardcoded to 0 */
+/* TODO: This should probably be called "void cacheEvents(void)"
+ * And executeOnData() should be int readEvents(data,count)
+ */
+int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) {
+
+
+    int lp_quaternion_on = 0, nbyte;
+    int i, mask = 0, numEventReceived = 0,
+        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
+            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
+    char *rdata = mIIOBuffer;
+
+    nbyte= (8 * sensors + 8) * 1;
+
+    if (isLowPowerQuatEnabled()) {
+        lp_quaternion_on = checkLPQuaternion();
+        if (lp_quaternion_on) {
+            nbyte += sizeof(mCachedQuaternionData);             //currently 16 bytes for Q data
+        }
+    }
+
+    ssize_t rsize = read(iio_fd, rdata, nbyte);
+    if (sensors == 0) {
+        rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
+    }
+
+#ifdef TESTING
+    LOGI("get one sample of IIO data with size: %d", rsize);
+    LOGI("sensors: %d", sensors);
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0)), *((short *) (rdata + 2)),
+        *((short *) (rdata + 4)));
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+
+    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
+        mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
+        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
+        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
+            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
+#endif
+
+    if (rsize != nbyte) {
+        LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)",
+             rsize, nbyte, sensors, errno, strerror(errno));
+        rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
+        return -1;
+    }
+
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
+
+        for (i=0; i< 4; i++) {
+            mCachedQuaternionData[i]= *(long*)rdata;
+            rdata += sizeof(long);
+        }
+    }
+
+    for (i = 0; i < 3; i++) {
+        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
+            mCachedGyroData[i] = *((short *) (rdata + i * 2));
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+            mCachedAccelData[i] = *((short *) (rdata + i * 2 +
+                ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
+        }
+        if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
+            mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
+        }
+    }
+
+    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
+        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
+    if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
+            (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
+        mask |= 1 << MagneticField;
+    }
+
+    mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
+    if (mCompassSensor->isIntegrated()) {
+        mCompassTimestamp = mSensorTimestamp;
+    }
+
+    if (mask & (1 << Gyro)) {
+        // send down temperature every 0.5 seconds
+        // with timestamp measured in "driver" layer
+        if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+            mTempCurrentTime = mSensorTimestamp;
+            long long temperature[2];
+            if(inv_read_temperature(temperature) == 0) {
+                LOGV_IF(INPUT_DATA,
+                        "HAL:inv_read_temperature = %lld, timestamp= %lld",
+                        temperature[0], temperature[1]);
+                inv_build_temp(temperature[0], temperature[1]);
+            }
+#ifdef TESTING
+            long bias[3], temp, temp_slope[3];
+            inv_get_gyro_bias(bias, &temp);
+            inv_get_gyro_ts(temp_slope);
+
+            LOGI("T: %.3f "
+                 "GB: %+13f %+13f %+13f "
+                 "TS: %+13f %+13f %+13f "
+                 "\n",
+                 (float)temperature[0] / 65536.f,
+                 (float)bias[0] / 65536.f / 16.384f,
+                 (float)bias[1] / 65536.f / 16.384f,
+                 (float)bias[2] / 65536.f / 16.384f,
+                 temp_slope[0] / 65536.f,
+                 temp_slope[1] / 65536.f,
+                 temp_slope[2] / 65536.f);
+#endif
+        }
+
+        mPendingMask |= 1 << Gyro;
+        mPendingMask |= 1 << RawGyro;
+
+        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
+            inv_build_gyro(mCachedGyroData, mSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+                    "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
+                    mCachedGyroData[0], mCachedGyroData[1],
+                    mCachedGyroData[2], mSensorTimestamp);
+        }
+    }
+
+    if (mask & (1 << Accelerometer)) {
+        mPendingMask |= 1 << Accelerometer;
+        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
+            inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
+             LOGV_IF(INPUT_DATA,
+                    "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+                    mCachedAccelData[0], mCachedAccelData[1],
+                    mCachedAccelData[2], mSensorTimestamp);
+        }
+    }
+
+    if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
+            inv_build_compass(mCachedCompassData, status,
+                              mCompassTimestamp);
+            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
+
+        inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
+        LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1],
+                    mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
+    }
+
+    return numEventReceived;
+}
+
+/* use for both MPUxxxx and third party compass */
+int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count)
+{
+    VHANDLER_LOG;
+
+    if (count < 1)
+        return -EINVAL;
+
+    int numEventReceived = 0;
+    int done = 0;
+
+    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+#ifdef COMPASS_YAS53x
+    if (mCompassSensor->checkCoilsReset()) {
+       //Reset relevant compass settings
+       resetCompass();
+    }
+#endif
+    if (done > 0) {
+        int status = 0;
+        if (mCompassSensor->providesCalibration()) {
+            status = mCompassSensor->getAccuracy();
+            status |= INV_CALIBRATED;
+        }
+        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
+            inv_build_compass(mCachedCompassData, status,
+                              mCompassTimestamp);
+            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
+                    mCachedCompassData[2], mCompassTimestamp);
+        }
+    }
+
+    return numEventReceived;
+}
+
+#ifdef COMPASS_YAS53x
+int MPLSensor::resetCompass()
+{
+    VFUNC_LOG;
+
+    //Reset compass cal if enabled
+    if (mFeatureActiveMask & INV_COMPASS_CAL) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+       inv_init_vector_compass_cal();
+    }
+
+    //Reset compass fit if enabled
+    if (mFeatureActiveMask & INV_COMPASS_FIT) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+       inv_init_compass_fit();
+    }
+
+    return 0;
+}
+#endif
+
+int MPLSensor::getFd() const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
+    return iio_fd;
+}
+
+int MPLSensor::getAccelFd() const
+{
+    VFUNC_LOG;
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
+    return accel_fd;
+}
+
+int MPLSensor::getCompassFd() const
+{
+    VFUNC_LOG;
+    int fd = mCompassSensor->getFd();
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
+    return fd;
+}
+
+int MPLSensor::turnOffAccelFifo() {
+    int i, res;
+    char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
+        mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
+
+    for (i = 0; i < 3; i++) {
+        res = write_sysfs_int(accel_fifo_enable[i], 0);
+        if (res < 0) {
+            return res;
+        }
+    }
+    return 0;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+    VFUNC_LOG;
+    /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpu3050())
+        return res;
+
+    pthread_mutex_lock(&GlobalHalMutex);
+
+    // on power if not already On
+    res = onPower(1);
+    // reset master enable
+    res = masterEnable(0);
+
+    if (en) {
+        //Enable DMP orientation
+        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android orientation");
+            res = -1; // indicate an err
+        }
+
+        // open DMP Orient Fd
+        res = openDmpOrientFd();
+
+        // enable DMP
+        res = onDMP(1);
+
+        // default DMP output rate to FIFO
+        if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
+            LOGE("HAL:ERR can't default DMP output rate");
+        }
+
+        // set DMP rate to 200Hz
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set DMP rate to 200Hz");
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+
+        // disable accel FIFO
+        if (!A_ENABLED) {
+            res = turnOffAccelFifo();
+        }
+
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
+    } else {
+        // disable DMP
+        res = onDMP(0);
+
+        // disable accel engine
+        if (!A_ENABLED) {
+            res = enableAccel(0);
+        }
+    }
+
+    res = masterEnable(1);
+    pthread_mutex_unlock(&GlobalHalMutex);
+    return res;
+}
+
+int MPLSensor::openDmpOrientFd()
+{
+    VFUNC_LOG;
+
+    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
+        return -1;
+    }
+
+    dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
+    if (dmp_orient_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmpOrient node");
+        return -1;
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
+        return 0;
+    }
+}
+
+int MPLSensor::closeDmpOrientFd()
+{
+    VFUNC_LOG;
+    if (dmp_orient_fd >= 0)
+        close(dmp_orient_fd);
+    return 0;
+}
+
+int MPLSensor::dmpOrientHandler(int orient)
+{
+    VFUNC_LOG;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
+    return 0;
+}
+
+int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
+    VFUNC_LOG;
+
+    char dummy[4];
+    int screen_orientation = 0;
+    FILE *fp;
+
+    fp = fopen(mpu.event_display_orientation, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_display_orientation");
+        return 0;
+    }
+    fscanf(fp, "%d\n", &screen_orientation);
+    fclose(fp);
+
+    int numEventReceived = 0;
+
+    if (mDmpOrientationEnabled && count > 0) {
+        sensors_event_t temp;
+
+        bzero(&temp, sizeof(temp));  /* Let's hope that SENSOR_TYPE_NONE is 0 */
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = ID_SO;
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
+        temp.screen_orientation = screen_orientation;
+#endif
+        struct timespec ts;
+        clock_gettime(CLOCK_MONOTONIC, &ts);
+        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+        *data++ = temp;
+        count--;
+        numEventReceived++;
+    }
+
+    // read dummy data per driver's request
+    dmpOrientHandler(screen_orientation);
+    read(dmp_orient_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpOrientFd()
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
+    return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation()
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+    if (checkDMPOrientation() || checkLPQuaternion()) {
+        // set DMP output rate to FIFO
+        write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
+
+        //DMP running rate must be @ 200Hz
+        *wanted= RATE_200HZ;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
+    }
+    return 0;
+}
+
+int MPLSensor::getPollTime()
+{
+    VHANDLER_LOG;
+    return mPollTime;
+}
+
+bool MPLSensor::hasPendingEvents() const
+{
+    VHANDLER_LOG;
+    // if we are using the polling workaround, force the main
+    // loop to check for data every time
+    return (mPollTime != -1);
+}
+
+/* TODO: support resume suspend when we gain more info about them*/
+void MPLSensor::sleepEvent()
+{
+    VFUNC_LOG;
+}
+
+void MPLSensor::wakeEvent()
+{
+    VFUNC_LOG;
+}
+
+int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (long)(fdata[0] * 65536.f);
+    ldata[1] = (long)(fdata[1] * 65536.f);
+    ldata[2] = (long)(fdata[2] * 65536.f);
+    return 0;
+}
+
+int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (fdata[1] * 65536.f);
+    ldata[1] = (fdata[2] * 65536.f);
+    ldata[2] = (fdata[3] * 65536.f);
+    return 0;
+}
+
+int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+            return -1;
+    ldata[0] = (long)fdata[0];
+    ldata[1] = (long)fdata[1];
+    ldata[2] = (long)fdata[2];
+    return 0;
+}
+
+int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
+{
+    VHANDLER_LOG;
+
+    if (!fdata || !ldata)
+        return -1;
+    ldata[0] = (short)fdata[0];
+    ldata[1] = (short)fdata[1];
+    ldata[2] = (short)fdata[2];
+    return 0;
+}
+
+int MPLSensor::inv_read_temperature(long long *data)
+{
+    VHANDLER_LOG;
+
+    int count = 0;
+    char raw_buf[40];
+    long raw = 0;
+
+    long long timestamp = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
+                                  sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading gyro temperature");
+        return -1;
+    }
+
+    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
+
+    if(count < 0) {
+        return -1;
+    }
+
+    LOGV_IF(ENG_VERBOSE,
+            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
+            raw, timestamp, count);
+    data[0] = raw;
+    data[1] = timestamp;
+
+    return 0;
+}
+
+int MPLSensor::inv_read_dmp_state(int fd)
+{
+    VFUNC_LOG;
+
+    if(fd < 0)
+        return -1;
+
+    int count = 0;
+    char raw_buf[10];
+    short raw = 0;
+
+    memset(raw_buf, 0, sizeof(raw_buf));
+    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+    if(count < 1) {
+        LOGE("HAL:error reading dmp state");
+        close(fd);
+        return -1;
+    }
+    count = sscanf(raw_buf, "%hd", &raw);
+    if(count < 0) {
+        LOGE("HAL:dmp state data is invalid");
+        close(fd);
+        return -1;
+    }
+    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
+    close(fd);
+    return (int)raw;
+}
+
+int MPLSensor::inv_read_sensor_bias(int fd, long *data)
+{
+    VFUNC_LOG;
+
+    if(fd == -1) {
+        return -1;
+    }
+
+    char buf[50];
+    char x[15], y[15], z[15];
+
+    memset(buf, 0, sizeof(buf));
+    int count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        LOGE("HAL:Error reading gyro bias");
+        return -1;
+    }
+    count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
+    if(count) {
+        /* scale appropriately for MPL */
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                atol(x), atol(y), atol(z));
+
+        data[0] = (long)(atol(x) / 10000 * (1L << 16));
+        data[1] = (long)(atol(y) / 10000 * (1L << 16));
+        data[2] = (long)(atol(z) / 10000 * (1L << 16));
+
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
+                data[0], data[1], data[2]);
+    }
+    return 0;
+}
+
+/** fill in the sensor list based on which sensors are configured.
+ *  return the number of configured sensors.
+ *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
+ *  parameter len gives the length of the buffer pointed to by list
+ */
+int MPLSensor::populateSensorList(struct sensor_t *list, int len)
+{
+    VFUNC_LOG;
+
+    int numsensors;
+
+    if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
+        LOGE("HAL:sensor list too small, not populating.");
+        return -(sizeof(sSensorList) / sizeof(sensor_t));
+    }
+
+    /* fill in the base values */
+    memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
+
+    /* first add gyro, accel and compass to the list */
+
+    /* fill in gyro/accel values */
+    fillGyro(chip_ID, list);
+    fillAccel(chip_ID, list);
+
+    // TODO: need fixes for unified HAL and 3rd-party solution
+    mCompassSensor->fillList(&list[MagneticField]);
+
+    if(1) {
+        numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
+        /* 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;
+}
+
+void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if (accel) {
+        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6050_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU9150_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
+            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+            list[Accelerometer].power = ACCEL_BMA250_POWER;
+            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown accel id %s -- "
+         "params default to bma250 and might be wrong.",
+         accel);
+    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
+    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
+    list[Accelerometer].power = ACCEL_BMA250_POWER;
+    list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
+}
+
+void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
+        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6050_POWER;
+        list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
+        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
+        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
+        list[Gyro].power = GYRO_MPU9150_POWER;
+        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
+    } else {
+        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
+        LOGE("HAL:default to use mpu3050 params");
+        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
+        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
+        list[Gyro].power = GYRO_MPU3050_POWER;
+        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
+    }
+
+    list[RawGyro].maxRange = list[Gyro].maxRange;
+    list[RawGyro].resolution = list[Gyro].resolution;
+    list[RawGyro].power = list[Gyro].power;
+    list[RawGyro].minDelay = list[Gyro].minDelay;
+
+    return;
+}
+
+/* fillRV depends on values of accel and compass in the list */
+void MPLSensor::fillRV(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    /* compute power on the fly */
+    list[RotationVector].power = list[Gyro].power +
+                                 list[Accelerometer].power +
+                                 list[MagneticField].power;
+    list[RotationVector].resolution = .00001;
+    list[RotationVector].maxRange = 1.0;
+    list[RotationVector].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillOrientation(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Orientation].power = list[Gyro].power +
+                              list[Accelerometer].power +
+                              list[MagneticField].power;
+    list[Orientation].resolution = .00001;
+    list[Orientation].maxRange = 360.0;
+    list[Orientation].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillGravity( struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[Gravity].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[Gravity].resolution = .00001;
+    list[Gravity].maxRange = 9.81;
+    list[Gravity].minDelay = 5000;
+
+    return;
+}
+
+void MPLSensor::fillLinearAccel(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    list[LinearAccel].power = list[Gyro].power +
+                          list[Accelerometer].power +
+                          list[MagneticField].power;
+    list[LinearAccel].resolution = list[Accelerometer].resolution;
+    list[LinearAccel].maxRange = list[Accelerometer].maxRange;
+    list[LinearAccel].minDelay = 5000;
+
+    return;
+}
+
+int MPLSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr =
+            (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr == NULL) {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    dptr = (char**)&mpu;
+    i = 0;
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
+        ALOGE("MPLSensor failed get sysfs path");
+        return -1;
+    }
+
+    if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
+        ALOGE("MPLSensor failed get iio trigger path");
+        return -1;
+    }
+
+    sprintf(mpu.key, "%s%s", sysfs_path, "/key");
+    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
+    sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
+
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
+    sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
+    sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+
+    // TODO: for self test
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+
+    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
+    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
+    sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
+    sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
+    sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
+    sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
+
+
+#ifndef THIRD_PARTY_ACCEL //MPUxxxx
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
+    // TODO: for bias settings
+    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
+#endif
+
+    sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
+    sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
+    sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+
+    sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
+    sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
+    sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
+    sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
+    sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
+
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+
+#if SYSFS_VERBOSE
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+/* TODO: stop manually testing/using 0 and 1 instead of
+ * false and true, but just use 0 and non-0.
+ * This allows  passing 0 and non-0 ints around instead of
+ * having to convert to 1 and test against 1.
+ */
+bool MPLSensor::isMpu3050()
+{
+    return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
+}
+
+int MPLSensor::isLowPowerQuatEnabled()
+{
+#ifdef ENABLE_LP_QUAT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn()
+{
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
diff --git a/60xx/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h
new file mode 100644
index 0000000..4698f33
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSensor.h
@@ -0,0 +1,346 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#ifndef ANDROID_MPL_SENSOR_H

+#define ANDROID_MPL_SENSOR_H

+

+#include <stdint.h>

+#include <errno.h>

+#include <sys/cdefs.h>

+#include <sys/types.h>

+#include <poll.h>

+#include <utils/Vector.h>

+#include <utils/KeyedVector.h>

+#include "sensors.h"

+#include "SensorBase.h"

+#include "InputEventReader.h"

+

+#ifdef INVENSENSE_COMPASS_CAL

+

+#ifdef COMPASS_YAS53x

+#pragma message "unified HAL for YAS53x"

+#include "CompassSensor.IIO.primary.h"

+#elif COMPASS_AMI306

+#pragma message "unified HAL for AMI306"

+#include "CompassSensor.IIO.primary.h"

+#else

+#pragma message "unified HAL for MPU9150"

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

+#endif

+#else

+#pragma message "unified HAL for AKM"

+#include "CompassSensor.AKM.h"

+#endif

+

+/*****************************************************************************/

+/* Sensors Enable/Disable Mask

+ *****************************************************************************/

+#define MAX_CHIP_ID_LEN             (20)

+

+#define INV_THREE_AXIS_GYRO         (0x000F)

+#define INV_THREE_AXIS_ACCEL        (0x0070)

+#define INV_THREE_AXIS_COMPASS      (0x0380)

+#define INV_ALL_SENSORS             (0x7FFF)

+

+#ifdef INVENSENSE_COMPASS_CAL

+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \

+                                      | INV_THREE_AXIS_COMPASS \

+                                      | INV_THREE_AXIS_GYRO)

+#else

+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \

+                                      | INV_THREE_AXIS_COMPASS \

+                                      | INV_THREE_AXIS_GYRO)

+#endif

+

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

+#define INV_COMPASS_CAL              0x01

+#define INV_COMPASS_FIT              0x02

+#define INV_DMP_QUATERNION           0x04

+#define INV_DMP_DISPL_ORIENTATION    0x08

+

+/* Uncomment to enable Low Power Quaternion */

+#define ENABLE_LP_QUAT_FEAT

+

+/* Uncomment to enable DMP display orientation 

+   (within the HAL, see below for Java framework) */

+// #define ENABLE_DMP_DISPL_ORIENT_FEAT

+

+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT

+/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION 

+   sensor type (DMP screen orientation) to the Java framework.

+   NOTE:

+       need Invensense customized 

+       'hardware/libhardware/include/hardware/sensors.h' to compile correctly.

+   NOTE:

+       need Invensense java framework changes to:

+       'frameworks/base/core/java/android/view/WindowOrientationListener.java'

+       'frameworks/base/core/java/android/hardware/Sensor.java'

+       'frameworks/base/core/java/android/hardware/SensorEvent.java'

+       for the 'Auto-rotate screen' to use this feature.

+*/

+#define ENABLE_DMP_SCREEN_AUTO_ROTATION

+#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"

+#endif

+

+int isDmpScreenAutoRotationEnabled()

+{

+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION

+    return 1;

+#else

+    return 0;

+#endif

+}

+

+int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;

+/*****************************************************************************/

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

+ *  by Google.

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

+ */

+

+class MPLSensor: public SensorBase

+{

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

+

+public:

+

+    enum {

+        Gyro = 0,

+        RawGyro,

+        Accelerometer,

+        MagneticField,

+        Orientation,

+        RotationVector,

+        LinearAccel,

+        Gravity,

+        numSensors

+    };

+

+    MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);

+    virtual ~MPLSensor();

+

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

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

+    int32_t getEnableMask() { return mEnabled; }

+

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

+    virtual int getFd() const;

+    virtual int getAccelFd() const;

+    virtual int getCompassFd() const;

+    virtual int getPollTime();

+    virtual bool hasPendingEvents() const;

+    virtual void sleepEvent();

+    virtual void wakeEvent();

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

+    void cbProcData();

+

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

+    static MPLSensor* gMPLSensor;

+

+    //AKM HAL Integration

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

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

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

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

+

+    int turnOffAccelFifo();

+    int enableDmpOrientation(int);

+    int dmpOrientHandler(int);

+    int readDmpOrientEvents(sensors_event_t* data, int count);

+    int getDmpOrientFd();

+    int openDmpOrientFd();

+    int closeDmpOrientFd();

+

+    int getDmpRate(int64_t *);

+    int checkDMPOrientation();

+

+protected:

+    CompassSensor *mCompassSensor;

+

+    int gyroHandler(sensors_event_t *data);

+    int rawGyroHandler(sensors_event_t *data);

+    int accelHandler(sensors_event_t *data);

+    int compassHandler(sensors_event_t *data);

+    int rvHandler(sensors_event_t *data);

+    int laHandler(sensors_event_t *data);

+    int gravHandler(sensors_event_t *data);

+    int orienHandler(sensors_event_t *data);

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

+    virtual int update_delay();

+

+    void inv_set_device_properties();

+    int inv_constructor_init();

+    int inv_constructor_default_enable();

+    int setGyroInitialState();

+    int setAccelInitialState();

+    int masterEnable(int en);

+    int onPower(int en);

+    int enableLPQuaternion(int);

+    int enableQuaternionData(int);

+    int onDMP(int);

+    int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int));

+    int enableGyro(int en);

+    int enableAccel(int en);

+    int enableCompass(int en);

+    void computeLocalSensorMask(int enabled_sensors);

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

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

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

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

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

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

+    int inv_read_temperature(long long *data);

+    int inv_read_dmp_state(int fd);

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

+    void inv_get_sensors_orientation(void);

+    int inv_init_sysfs_attributes(void);

+#ifdef COMPASS_YAS53x

+    int resetCompass(void);

+#endif

+    void setCompassDelay(int64_t ns);

+    void enable_iio_sysfs(void);

+    int enableTap(int);

+    int enableFlick(int);

+    int enablePedometer(int);

+    int checkLPQuaternion();

+

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

+    int mDmpStarted;

+    long mMasterSensorMask;

+    long mLocalSensorMask;

+    int mPollTime;

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

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

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

+    int mCompassAccuracy;     // value indicating the quality of the compass calibr.

+    struct pollfd mPollFds[5];

+    int mSampleCount;

+    pthread_mutex_t mMplMutex;

+    pthread_mutex_t mHALMutex;

+

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

+

+    int iio_fd;

+    int accel_fd;

+    int mpufifo_fd;

+    int gyro_temperature_fd;

+    int dmp_orient_fd;

+

+    int mDmpOrientationEnabled;

+

+

+    uint32_t mEnabled;

+    uint32_t mOldEnabledMask;

+    sensors_event_t mPendingEvents[numSensors];

+    int64_t mDelays[numSensors];

+    hfunc_t mHandlers[numSensors];

+    short mCachedGyroData[3];

+    long mCachedAccelData[3];

+    long mCachedCompassData[3];

+    long mCachedQuaternionData[4];

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

+

+    InputEventCircularReader mAccelInputReader;

+    InputEventCircularReader mGyroInputReader;

+

+    bool mFirstRead;

+    short mTempScale;

+    short mTempOffset;

+    int64_t mTempCurrentTime;

+    int mAccelScale;

+

+    uint32_t mPendingMask;

+    unsigned long mSensorMask;

+

+    char chip_ID[MAX_CHIP_ID_LEN];

+

+    signed char mGyroOrientation[9];

+    signed char mAccelOrientation[9];

+

+    int64_t mSensorTimestamp;

+    int64_t mCompassTimestamp;

+

+    struct sysfs_attrbs {

+       char *chip_enable;

+       char *power_state;

+       char *dmp_firmware;

+       char *firmware_loaded;

+       char *dmp_on;

+       char *dmp_int_on;

+       char *dmp_event_int_on;

+       char *dmp_output_rate;

+       char *tap_on;

+       char *key;

+       char *self_test;

+       char *temperature;

+

+       char *gyro_enable;

+       char *gyro_fifo_rate;

+       char *gyro_orient;

+       char *gyro_x_fifo_enable;

+       char *gyro_y_fifo_enable;

+       char *gyro_z_fifo_enable;

+

+       char *accel_enable;

+       char *accel_fifo_rate;

+       char *accel_fsr;

+       char *accel_bias;

+       char *accel_orient;

+       char *accel_x_fifo_enable;

+       char *accel_y_fifo_enable;

+       char *accel_z_fifo_enable;

+

+       char *quaternion_on;

+       char *in_quat_r_en;

+       char *in_quat_x_en;

+       char *in_quat_y_en;

+       char *in_quat_z_en;

+

+       char *in_timestamp_en;

+       char *trigger_name;

+       char *current_trigger;

+       char *buffer_length;

+

+       char *display_orientation_on;

+       char *event_display_orientation;

+    } mpu;

+

+    char *sysfs_names_ptr;

+    int mFeatureActiveMask;

+

+private:

+    /* added for dynamic get sensor list */

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

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

+    void fillRV(struct sensor_t *list);

+    void fillOrientation(struct sensor_t *list);

+    void fillGravity(struct sensor_t *list);

+    void fillLinearAccel(struct sensor_t *list);

+    void storeCalibration();

+    void loadDMP();

+    bool isMpu3050();

+    int isLowPowerQuatEnabled();

+    int isDmpDisplayOrientationOn();

+

+

+};

+

+extern "C" {

+    void setCallbackObject(MPLSensor*);

+    MPLSensor *getCallbackObject();

+}

+

+#endif  // ANDROID_MPL_SENSOR_H

diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp
new file mode 100644
index 0000000..9773562
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSupport.cpp
@@ -0,0 +1,190 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+#include <MPLSupport.h>
+#include <string.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "log.h"
+#include "SensorBase.h"
+
+#include "ml_sysfs_helper.h"
+#include "local_log_def.h"
+
+int64_t getTimestamp()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
+int64_t timevalToNano(timeval const& t) {
+    return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+}
+
+int inv_read_data(char *fname, long *data)
+{
+    VFUNC_LOG;
+
+    char buf[sizeof(long) * 4];
+    int count, fd;
+
+    fd = open(fname, O_RDONLY);
+    if(fd < 0) {
+        LOGE("HAL:Error opening %s", fname);
+        return -1;
+    }
+    memset(buf, 0, sizeof(buf));
+    count = read_attribute_sensor(fd, buf, sizeof(buf));
+    if(count < 1) {
+        close(fd);
+        return -1;
+    } else {
+        count = sscanf(buf, "%ld", data);
+        if(count)
+            LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
+    }
+    close(fd);
+
+    return 0;
+}
+
+/* This one DOES NOT close FDs for you */
+int read_attribute_sensor(int fd, char* data, unsigned int size)
+{
+    VFUNC_LOG;
+
+    int count = 0;
+    if (fd > 0) {
+        count = pread(fd, data, size, 0);
+        if(count < 1) {
+            LOGE("HAL:read fails with error code=%d", count);
+        }
+    }
+    return count;
+}
+
+/**
+ *  @brief  Enable a sensor through the sysfs file descriptor
+ *          provided.
+ *  @note   this function one closes FD after the write
+ *  @param  fd
+ *              the file descriptor to write into
+ *  @param  en
+ *              the value to write, typically 1 or 0
+ *  @return the errno whenever applicable.
+ */
+int enable_sysfs_sensor(int fd, int en)
+{
+    VFUNC_LOG;
+
+    int nb;
+    int err = 0;
+
+    char c = en ? '1' : '0';
+    nb = write(fd, &c, 1);
+
+    if (nb <= 0) {
+        err = errno;
+        LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
+             c, nb, strerror(err), err);
+    }
+    close(fd);
+
+
+    return err;
+}
+
+/* This one closes FDs for you */
+int write_attribute_sensor(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+        close(fd);
+    }
+
+    return num_b;
+}
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE  *sysfsfp;
+
+    sysfsfp = fopen(filename, "r");
+    if (sysfsfp != NULL) {
+        if (fscanf(sysfsfp, "%d\n", var) < 1) {
+           LOGE("HAL:ERR failed to read an int from %s.", filename);
+           res = -EINVAL;
+        }
+        fclose(sysfsfp);
+    } else {
+        res = -errno;
+        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int var)
+{
+    int res = 0;
+    FILE  *sysfsfp;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            var, filename, getTimestamp());
+    sysfsfp = fopen(filename, "w");
+    if (sysfsfp == NULL) {
+        res = -errno;
+        LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+        return res;
+    }
+    int fpres, fcres = 0;
+    fpres = fprintf(sysfsfp, "%d\n", var);
+    /* fprintf() can succeed because it never actually writes to the
+     * underlying sysfs node.
+     */
+    if (fpres < 0) {
+       res = -errno;
+       fclose(sysfsfp);
+    } else {
+        fcres = fclose(sysfsfp);
+        /* Check for errors from: fflush(), write(), and close() */
+        if (fcres < 0) {
+            res = -errno;
+        }
+    }
+    if (fpres < 0 || fcres < 0) {
+        LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d",
+            var, filename, res, fpres, fcres);
+    }
+    return res;
+}
diff --git a/60xx/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h
new file mode 100644
index 0000000..98e4497
--- /dev/null
+++ b/60xx/libsensors_iio/MPLSupport.h
@@ -0,0 +1,33 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SUPPORT_H
+#define ANDROID_MPL_SUPPORT_H
+
+#include <stdint.h>
+#include <time.h>
+
+int64_t getTimestamp();
+int64_t timevalToNano(timeval const& t);
+
+int inv_read_data(char *fname, long *data);
+int read_attribute_sensor(int fd, char* data, unsigned int size);
+int enable_sysfs_sensor(int fd, int en);
+int write_attribute_sensor(int fd, long data);
+int read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
+
+#endif //  ANDROID_MPL_SUPPORT_H
diff --git a/60xx/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp
new file mode 100644
index 0000000..587aaf5
--- /dev/null
+++ b/60xx/libsensors_iio/SensorBase.cpp
@@ -0,0 +1,135 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#include <fcntl.h>

+#include <errno.h>

+#include <math.h>

+#include <poll.h>

+#include <dirent.h>

+#include <string.h>

+#include <sys/select.h>

+#include <cutils/log.h>

+#include <linux/input.h>

+

+#include "SensorBase.h"

+#include "local_log_def.h"

+

+/*****************************************************************************/

+

+SensorBase::SensorBase(const char* dev_name,

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

+                                                data_name(data_name),

+                                                dev_fd(-1),

+                                                data_fd(-1)

+{

+    if (data_name) {

+        data_fd = openInput(data_name);

+    }

+}

+

+SensorBase::~SensorBase()

+{

+    if (data_fd >= 0) {

+        close(data_fd);

+    }

+    if (dev_fd >= 0) {

+        close(dev_fd);

+    }

+}

+

+int SensorBase::open_device()

+{

+    if (dev_fd<0 && dev_name) {

+        dev_fd = open(dev_name, O_RDONLY);

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

+    }

+    return 0;

+}

+

+int SensorBase::close_device()

+{

+    if (dev_fd >= 0) {

+        close(dev_fd);

+        dev_fd = -1;

+    }

+    return 0;

+}

+

+int SensorBase::getFd() const

+{

+    if (!data_name) {

+        return dev_fd;

+    }

+    return data_fd;

+}

+

+int SensorBase::setDelay(int32_t /*handle*/, int64_t /*ns*/)

+{

+    return 0;

+}

+

+bool SensorBase::hasPendingEvents() const

+{

+    return false;

+}

+

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

+{

+    int fd = -1;

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

+    char devname[PATH_MAX];

+    char *filename;

+    DIR *dir;

+    struct dirent *de;

+    dir = opendir(dirname);

+    if(dir == NULL)

+        return -1;

+    strcpy(devname, dirname);

+    filename = devname + strlen(devname);

+    *filename++ = '/';

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

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

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

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

+            continue;

+        strcpy(filename, de->d_name);

+        fd = open(devname, O_RDONLY);

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

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

+        if (fd >= 0) {

+            char name[80];

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

+                name[0] = '\0';

+            }

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

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

+                strcpy(input_name, filename);

+                break;

+            } else {

+                close(fd);

+                fd = -1;

+            }

+        }

+    }

+    closedir(dir);

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

+    return fd;

+}

+

+int SensorBase::enable(int32_t /*handle*/, int /*enabled*/)

+{

+    return 0;

+}

diff --git a/60xx/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h
new file mode 100644
index 0000000..fef47c7
--- /dev/null
+++ b/60xx/libsensors_iio/SensorBase.h
@@ -0,0 +1,61 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSOR_BASE_H
+#define ANDROID_SENSOR_BASE_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define IIO_BUFFER_LENGTH   (480)
+
+/*****************************************************************************/
+
+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);
+};
+
+/*****************************************************************************/
+
+#endif  // ANDROID_SENSOR_BASE_H
diff --git a/60xx/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so
new file mode 100755
index 0000000..9bdd5bc
--- /dev/null
+++ b/60xx/libsensors_iio/libmllite.so
diff --git a/60xx/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so
new file mode 100644
index 0000000..205ab9a
--- /dev/null
+++ b/60xx/libsensors_iio/libmplmpu.so
diff --git a/60xx/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h
new file mode 100644
index 0000000..9135992
--- /dev/null
+++ b/60xx/libsensors_iio/local_log_def.h
@@ -0,0 +1,49 @@
+#ifndef LOCAL_LOG_DEF_H

+#define LOCAL_LOG_DEF_H

+

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

+

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

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

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

+                               purpose on a shell */

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

+

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

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

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

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

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

+

+#if defined ANDROID_JELLYBEAN

+#define LOGV            ALOGV

+#define LOGV_IF         ALOGV_IF

+#define LOGD            ALOGD

+#define LOGD_IF         ALOGD_IF

+#define LOGI            ALOGI

+#define LOGI_IF         ALOGI_IF

+#define LOGW            ALOGW

+#define LOGW_IF         ALOGW_IF

+#define LOGE            ALOGE

+#define LOGE_IF         ALOGE_IF

+#define IF_LOGV         IF_ALOGV

+#define IF_LOGD         IF_ALOGD

+#define IF_LOGI         IF_ALOGI

+#define IF_LOGW         IF_ALOGW

+#define IF_LOGE         IF_ALOGE

+#define LOG_ASSERT      ALOG_ASSERT

+#define LOG                     ALOG

+#define IF_LOG          IF_ALOG

+#else

+#warning "build for ICS or earlier version"

+#endif

+

+

+#define FUNC_LOG \

+            LOGV("%s", __PRETTY_FUNCTION__)

+#define VFUNC_LOG \

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

+#define VHANDLER_LOG \

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

+

+#endif /*ifndef LOCAL_LOG_DEF_H */

diff --git a/60xx/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h
new file mode 100644
index 0000000..eef0b3b
--- /dev/null
+++ b/60xx/libsensors_iio/sensor_params.h
@@ -0,0 +1,194 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#ifndef INV_SENSOR_PARAMS_H

+#define INV_SENSOR_PARAMS_H

+

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

+#define SENSORS_ROTATION_VECTOR_HANDLE    (ID_RV)

+#define SENSORS_LINEAR_ACCEL_HANDLE       (ID_LA)

+#define SENSORS_GRAVITY_HANDLE            (ID_GR)

+#define SENSORS_GYROSCOPE_HANDLE          (ID_GY)

+#define SENSORS_RAW_GYROSCOPE_HANDLE      (ID_RG)

+#define SENSORS_ACCELERATION_HANDLE       (ID_A)

+#define SENSORS_MAGNETIC_FIELD_HANDLE     (ID_M)

+#define SENSORS_ORIENTATION_HANDLE        (ID_O)

+#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)

+

+/******************************************/

+//MPU9250 INV_COMPASS

+#define COMPASS_MPU9250_RANGE           (9830.f)

+#define COMPASS_MPU9250_RESOLUTION      (0.15f)

+#define COMPASS_MPU9250_POWER           (10.f)

+#define COMPASS_MPU9250_MINDELAY        (10000)

+//MPU9150 INV_COMPASS

+#define COMPASS_MPU9150_RANGE           (9830.f)

+#define COMPASS_MPU9150_RESOLUTION      (0.285f)

+#define COMPASS_MPU9150_POWER           (10.f)

+#define COMPASS_MPU9150_MINDELAY        (10000)

+//COMPASS_ID_AK8975

+#define COMPASS_AKM8975_RANGE           (9830.f)

+#define COMPASS_AKM8975_RESOLUTION      (0.285f)

+#define COMPASS_AKM8975_POWER           (10.f)

+#define COMPASS_AKM8975_MINDELAY        (10000)

+//COMPASS_ID_AK8963C

+#define COMPASS_AKM8963_RANGE           (9830.f)

+#define COMPASS_AKM8963_RESOLUTION      (0.15f)

+#define COMPASS_AKM8963_POWER           (10.f)

+#define COMPASS_AKM8963_MINDELAY        (10000)

+//COMPASS_ID_AMI30X

+#define COMPASS_AMI30X_RANGE            (5461.f)

+#define COMPASS_AMI30X_RESOLUTION       (0.9f)

+#define COMPASS_AMI30X_POWER            (0.15f)

+//COMPASS_ID_AMI306

+#define COMPASS_AMI306_RANGE            (5461.f)

+#define COMPASS_AMI306_RESOLUTION       (0.9f)

+#define COMPASS_AMI306_POWER            (0.15f)

+#define COMPASS_AMI306_MINDELAY         (10000)

+//COMPASS_ID_YAS529

+#define COMPASS_YAS529_RANGE            (19660.f)

+#define COMPASS_YAS529_RESOLUTION       (0.012f)

+#define COMPASS_YAS529_POWER            (4.f)

+//COMPASS_ID_YAS53x

+#define COMPASS_YAS53x_RANGE            (8001.f)

+#define COMPASS_YAS53x_RESOLUTION       (0.012f)

+#define COMPASS_YAS53x_POWER            (4.f)

+#define COMPASS_YAS53x_MINDELAY         (10000)

+//COMPASS_ID_HMC5883

+#define COMPASS_HMC5883_RANGE           (10673.f)

+#define COMPASS_HMC5883_RESOLUTION      (10.f)

+#define COMPASS_HMC5883_POWER           (0.24f)

+//COMPASS_ID_LSM303DLH

+#define COMPASS_LSM303DLH_RANGE         (10240.f)

+#define COMPASS_LSM303DLH_RESOLUTION    (1.f)

+#define COMPASS_LSM303DLH_POWER         (1.f)

+//COMPASS_ID_LSM303DLM

+#define COMPASS_LSM303DLM_RANGE         (10240.f)

+#define COMPASS_LSM303DLM_RESOLUTION    (1.f)

+#define COMPASS_LSM303DLM_POWER         (1.f)

+//COMPASS_ID_MMC314X

+#define COMPASS_MMC314X_RANGE           (400.f)

+#define COMPASS_MMC314X_RESOLUTION      (2.f)

+#define COMPASS_MMC314X_POWER           (0.55f)

+//COMPASS_ID_HSCDTD002B

+#define COMPASS_HSCDTD002B_RANGE        (9830.f)

+#define COMPASS_HSCDTD002B_RESOLUTION   (1.f)

+#define COMPASS_HSCDTD002B_POWER        (1.f)

+//COMPASS_ID_HSCDTD004A

+#define COMPASS_HSCDTD004A_RANGE        (9830.f)

+#define COMPASS_HSCDTD004A_RESOLUTION   (1.f)

+#define COMPASS_HSCDTD004A_POWER        (1.f)

+/*******************************************/

+//ACCEL_ID_MPU6500

+#define ACCEL_MPU6500_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MPU6500_RESOLUTION        (0.004f * GRAVITY_EARTH)

+#define ACCEL_MPU6500_POWER             (0.f)

+#define ACCEL_MPU6500_MINDELAY          (1000)

+//ACCEL_ID_MPU9250

+#define ACCEL_MPU9250_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MPU9250_RESOLUTION        (0.004f * GRAVITY_EARTH)

+#define ACCEL_MPU9250_POWER             (0.f)

+#define ACCEL_MPU9250_MINDELAY          (1000)

+//ACCEL_ID_MPU9150

+#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)

+#define ACCEL_MPU9150_POWER             (0.f)

+#define ACCEL_MPU9150_MINDELAY          (1000)

+//ACCEL_ID_LIS331

+#define ACCEL_LIS331_RANGE              (2.48f * GRAVITY_EARTH)

+#define ACCEL_LIS331_RESOLUTION         (0.001f * GRAVITY_EARTH)

+#define ACCEL_LIS331_POWER              (1.f)

+//ACCEL_ID_LSM303DLX

+#define ACCEL_LSM303DLX_RANGE           (2.48f * GRAVITY_EARTH)

+#define ACCEL_LSM303DLX_RESOLUTION      (0.001f * GRAVITY_EARTH)

+#define ACCEL_LSM303DLX_POWER           (1.f)

+//ACCEL_ID_LIS3DH

+#define ACCEL_LIS3DH_RANGE              (2.48f * GRAVITY_EARTH)

+#define ACCEL_LIS3DH_RESOLUTION         (0.001f * GRAVITY_EARTH)

+#define ACCEL_LIS3DH_POWER              (1.f)

+//ACCEL_ID_KXSD9

+#define ACCEL_KXSD9_RANGE               (2.5006f * GRAVITY_EARTH)

+#define ACCEL_KXSD9_RESOLUTION          (0.001f * GRAVITY_EARTH)

+#define ACCEL_KXSD9_POWER               (1.f)

+//ACCEL_ID_KXTF9

+#define ACCEL_KXTF9_RANGE               (1.f * GRAVITY_EARTH)

+#define ACCEL_KXTF9_RESOLUTION          (0.033f * GRAVITY_EARTH)

+#define ACCEL_KXTF9_POWER               (0.35f)

+//ACCEL_ID_BMA150

+#define ACCEL_BMA150_RANGE              (2.f * GRAVITY_EARTH)

+#define ACCEL_BMA150_RESOLUTION         (0.004f * GRAVITY_EARTH)

+#define ACCEL_BMA150_POWER              (0.2f)

+//ACCEL_ID_BMA222

+#define ACCEL_BMA222_RANGE              (2.f * GRAVITY_EARTH)

+#define ACCEL_BMA222_RESOLUTION         (0.001f * GRAVITY_EARTH)

+#define ACCEL_BMA222_POWER              (0.1f)

+//ACCEL_ID_BMA250

+#define ACCEL_BMA250_RANGE              (2.f * GRAVITY_EARTH)

+#define ACCEL_BMA250_RESOLUTION         (0.00391f * GRAVITY_EARTH)

+#define ACCEL_BMA250_POWER              (0.139f)

+#define ACCEL_BMA250_MINDELAY           (1000)

+//ACCEL_ID_ADXL34X

+#define ACCEL_ADXL34X_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_ADXL34X_RESOLUTION        (0.001f * GRAVITY_EARTH)

+#define ACCEL_ADXL34X_POWER             (1.f)

+//ACCEL_ID_MMA8450

+#define ACCEL_MMA8450_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MMA8450_RESOLUTION        (0.001f * GRAVITY_EARTH)

+#define ACCEL_MMA8450_POWER             (1.0f)

+//ACCEL_ID_MMA845X

+#define ACCEL_MMA845X_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MMA845X_RESOLUTION        (0.001f * GRAVITY_EARTH)

+#define ACCEL_MMA845X_POWER             (1.f)

+//ACCEL_ID_MPU6050

+#define ACCEL_MPU6050_RANGE             (2.f * GRAVITY_EARTH)

+#define ACCEL_MPU6050_RESOLUTION        (0.004f * GRAVITY_EARTH)

+#define ACCEL_MPU6050_POWER             (0.f)

+#define ACCEL_MPU6050_MINDELAY          (5000)

+/******************************************/

+//GYRO MPU3050

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

+#define GYRO_MPU3050_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_MPU3050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_MPU3050_POWER              (6.1f)

+#define GYRO_MPU3050_MINDELAY           (1000)

+//GYRO MPU6050

+#define GYRO_MPU6050_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_MPU6050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_MPU6050_POWER              (5.5f)

+#define GYRO_MPU6050_MINDELAY           (5000)

+//GYRO MPU9150

+#define GYRO_MPU9150_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_MPU9150_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_MPU9150_POWER              (5.5f)

+#define GYRO_MPU9150_MINDELAY           (1000)

+//GYRO MPU9250

+#define GYRO_MPU9250_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_MPU9250_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_MPU9250_POWER              (5.5f)

+#define GYRO_MPU9250_MINDELAY           (1000)

+//GYRO MPU6500

+#define GYRO_MPU6500_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_MPU6500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_MPU6500_POWER              (5.5f)

+#define GYRO_MPU6500_MINDELAY           (1000)

+//GYRO ITG3500

+#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)

+#define GYRO_ITG3500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)

+#define GYRO_ITG3500_POWER              (5.5f)

+#define GYRO_ITG3500_MINDELAY           (1000)

+

+#endif  /* INV_SENSOR_PARAMS_H */

+

diff --git a/60xx/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h
new file mode 100644
index 0000000..f698fc5
--- /dev/null
+++ b/60xx/libsensors_iio/sensors.h
@@ -0,0 +1,97 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

+#ifndef ANDROID_SENSORS_H

+#define ANDROID_SENSORS_H

+

+#include <stdint.h>

+#include <errno.h>

+#include <sys/cdefs.h>

+#include <sys/types.h>

+

+#include <linux/input.h>

+

+#include <hardware/hardware.h>

+#include <hardware/sensors.h>

+

+__BEGIN_DECLS

+

+/*****************************************************************************/

+

+#ifndef ARRAY_SIZE

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

+#endif

+

+enum {

+    ID_GY = 0,

+    ID_RG,

+    ID_A,

+    ID_M,

+    ID_O,

+    ID_RV,

+    ID_LA,

+    ID_GR,

+    ID_SO

+};

+

+/*****************************************************************************/

+

+/*

+ * The SENSORS Module

+ */

+

+/* ITG3500 */

+#define EVENT_TYPE_GYRO_X          REL_X

+#define EVENT_TYPE_GYRO_Y          REL_Y

+#define EVENT_TYPE_GYRO_Z          REL_Z

+/* MPU6050 MPU9150 */

+#define EVENT_TYPE_IACCEL_X        REL_RX

+#define EVENT_TYPE_IACCEL_Y        REL_RY

+#define EVENT_TYPE_IACCEL_Z        REL_RZ

+/* MPU6050 MPU9150 */

+#define EVENT_TYPE_ICOMPASS_X      REL_X

+#define EVENT_TYPE_ICOMPASS_Y      REL_Y

+#define EVENT_TYPE_ICOMPASS_Z      REL_Z

+/* MPUxxxx */

+#define EVENT_TYPE_TIMESTAMP_HI    REL_MISC

+#define EVENT_TYPE_TIMESTAMP_LO    REL_WHEEL

+

+/* Accel BMA250 */

+#define EVENT_TYPE_ACCEL_X          ABS_X

+#define EVENT_TYPE_ACCEL_Y          ABS_Y

+#define EVENT_TYPE_ACCEL_Z          ABS_Z

+#define LSG                         (1000.0f)

+

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

+#define RANGE_A                     (4*GRAVITY_EARTH)

+#define RESOLUTION_A                (GRAVITY_EARTH / LSG)

+#define CONVERT_A                   (GRAVITY_EARTH / LSG)

+#define CONVERT_A_X                 (CONVERT_A)

+#define CONVERT_A_Y                 (CONVERT_A)

+#define CONVERT_A_Z                 (CONVERT_A)

+

+/* Compass AKM8975 */

+#define EVENT_TYPE_MAGV_X           ABS_RX

+#define EVENT_TYPE_MAGV_Y           ABS_RY

+#define EVENT_TYPE_MAGV_Z           ABS_RZ

+#define EVENT_TYPE_MAGV_STATUS      ABS_RUDDER

+

+// conversion of magnetic data to uT units

+#define CONVERT_M                   (0.06f)

+

+__END_DECLS

+

+#endif  // ANDROID_SENSORS_H

diff --git a/60xx/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp
new file mode 100644
index 0000000..4aef514
--- /dev/null
+++ b/60xx/libsensors_iio/sensors_mpl.cpp
@@ -0,0 +1,260 @@
+/*

+* Copyright (C) 2012 Invensense, Inc.

+*

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

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

+* You may obtain a copy of the License at

+*

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

+*

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

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

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

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

+* limitations under the License.

+*/

+

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

+

+#include <hardware/sensors.h>

+#include <fcntl.h>

+#include <errno.h>

+#include <dirent.h>

+#include <math.h>

+#include <poll.h>

+#include <pthread.h>

+#include <stdlib.h>

+

+#include <linux/input.h>

+

+#include <utils/Atomic.h>

+#include <utils/Log.h>

+

+#include "sensors.h"

+#include "MPLSensor.h"

+

+/*****************************************************************************/

+/* The SENSORS Module */

+

+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION

+#define LOCAL_SENSORS (MPLSensor::numSensors + 1)

+#else

+#define LOCAL_SENSORS MPLSensor::numSensors

+#endif

+

+/* Vendor-defined Accel Load Calibration File Method

+* @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame

+* @return '0' for a successful load, '1' otherwise

+* example: int AccelLoadConfig(long* offset);

+* End of Vendor-defined Accel Load Cal Method

+*/

+

+static struct sensor_t sSensorList[LOCAL_SENSORS];

+static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));

+

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

+                        struct hw_device_t** device);

+

+static int sensors__get_sensors_list(struct sensors_module_t* module,

+                                     struct sensor_t const** list)

+{

+    *list = sSensorList;

+    return sensors;

+}

+

+static struct hw_module_methods_t sensors_module_methods = {

+        open: open_sensors

+};

+

+struct sensors_module_t HAL_MODULE_INFO_SYM = {

+        common: {

+                tag: HARDWARE_MODULE_TAG,

+                version_major: 1,

+                version_minor: 0,

+                id: SENSORS_HARDWARE_MODULE_ID,

+                name: "Invensense module",

+                author: "Invensense Inc.",

+                methods: &sensors_module_methods,

+        },

+        get_sensors_list: sensors__get_sensors_list,

+};

+

+struct sensors_poll_context_t {

+    struct sensors_poll_device_t device; // must be first

+

+    sensors_poll_context_t();

+    ~sensors_poll_context_t();

+    int activate(int handle, int enabled);

+    int setDelay(int handle, int64_t ns);

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

+

+private:

+    enum {

+        mpl = 0,

+        compass,

+        dmpOrient,

+        numSensorDrivers,   // wake pipe goes here

+        numFds,

+    };

+

+    struct pollfd mPollFds[numSensorDrivers];

+    SensorBase *mSensor;

+};

+

+/******************************************************************************/

+

+sensors_poll_context_t::sensors_poll_context_t() {

+    VFUNC_LOG;

+

+    mCompassSensor = new CompassSensor();

+    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);

+

+   /* For Vendor-defined Accel Calibration File Load

+    * Use the Following Constructor and Pass Your Load Cal File Function

+    *

+    * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);

+    */

+

+    // setup the callback object for handing mpl callbacks

+    setCallbackObject(mplSensor);

+

+    // populate the sensor list

+    sensors =

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

+

+    mSensor = mplSensor;

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

+    mPollFds[mpl].events = POLLIN;

+    mPollFds[mpl].revents = 0;

+

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

+    mPollFds[compass].events = POLLIN;

+    mPollFds[compass].revents = 0;

+

+    mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();

+    mPollFds[dmpOrient].events = POLLPRI;

+    mPollFds[dmpOrient].revents = 0;

+}

+

+sensors_poll_context_t::~sensors_poll_context_t() {

+    FUNC_LOG;

+    delete mSensor;

+}

+

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

+    FUNC_LOG;

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

+}

+

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

+{

+    FUNC_LOG;

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

+}

+

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

+{

+    VHANDLER_LOG;

+

+    int nbEvents = 0;

+    int nb, polltime = -1;

+

+    // look for new events

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

+

+    if (nb > 0) {

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

+            if (mPollFds[i].revents & (POLLIN | POLLPRI)) {

+                nb = 0;

+                if (i == mpl) {

+                    /* Ignore res */

+                    mSensor->readEvents(NULL, 0);

+                    mPollFds[i].revents = 0;

+                }

+                else if (i == compass) {

+                    /* Ignore res */

+                    ((MPLSensor*) mSensor)->readCompassEvents(NULL, count);

+                    mPollFds[i].revents = 0;

+                }

+            }

+        }

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

+        if (nb > 0) {

+            count -= nb;

+            nbEvents += nb;

+            data += nb;

+        }

+

+        if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {

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

+            mPollFds[dmpOrient].revents= 0;

+            if (isDmpScreenAutoRotationEnabled() && nb > 0) {

+                count -= nb;

+                nbEvents += nb;

+                data += nb;

+            }

+        }

+    }

+

+    return nbEvents;

+}

+

+/******************************************************************************/

+

+static int poll__close(struct hw_device_t *dev)

+{

+    FUNC_LOG;

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

+    if (ctx) {

+        delete ctx;

+    }

+    return 0;

+}

+

+static int poll__activate(struct sensors_poll_device_t *dev,

+                          int handle, int enabled)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

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

+}

+

+static int poll__setDelay(struct sensors_poll_device_t *dev,

+                          int handle, int64_t ns)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

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

+    return s;

+}

+

+static int poll__poll(struct sensors_poll_device_t *dev,

+                      sensors_event_t* data, int count)

+{

+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

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

+}

+

+/******************************************************************************/

+

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

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

+                        struct hw_device_t** device)

+{

+    FUNC_LOG;

+    int status = -EINVAL;

+    sensors_poll_context_t *dev = new sensors_poll_context_t();

+

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

+

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

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

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

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

+    dev->device.activate        = poll__activate;

+    dev->device.setDelay        = poll__setDelay;

+    dev->device.poll            = poll__poll;

+

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

+    status = 0;

+

+    return status;

+}

diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk
new file mode 100644
index 0000000..84e7e9b
--- /dev/null
+++ b/60xx/libsensors_iio/software/build/android/common.mk
@@ -0,0 +1,87 @@
+# Use bash for additional echo fancyness
+SHELL = /bin/bash
+
+####################################################################################################
+## defines
+
+# Build for Jellybean 
+BUILD_ANDROID_JELLYBEAN = 1
+
+## libraries ##
+LIB_PREFIX = lib
+
+STATIC_LIB_EXT = a
+SHARED_LIB_EXT = so
+
+# normally, overridden from outside 
+# ?= assignment sets it only if not already defined
+TARGET ?= android
+
+MLLITE_LIB_NAME     ?= mllite
+MPL_LIB_NAME        ?= mplmpu
+
+## applications ##
+SHARED_APP_SUFFIX = -shared
+STATIC_APP_SUFFIX = -static
+
+####################################################################################################
+## compile, includes, and linker
+
+ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
+endif
+
+ANDROID_LINK  = -nostdlib
+ANDROID_LINK += -fpic
+ANDROID_LINK += -Wl,--gc-sections 
+ANDROID_LINK += -Wl,--no-whole-archive 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+ANDROID_LINK_EXECUTABLE  = $(ANDROID_LINK)
+ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
+ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+endif
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+
+ANDROID_INCLUDES  = -I$(ANDROID_ROOT)/system/core/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include   # ICS
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates
+
+KERNEL_INCLUDES  = -I$(KERNEL_ROOT)/include
+
+INV_INCLUDES  = -I$(INV_ROOT)/software/core/driver/include
+INV_INCLUDES += -I$(MLLITE_DIR)
+INV_INCLUDES += -I$(MLLITE_DIR)/linux
+
+INV_DEFINES += -DINV_CACHE_DMP=1
+
+####################################################################################################
+## macros
+
+ifndef echo_in_colors
+define echo_in_colors
+	echo -ne "\e[1;32m"$(1)"\e[0m"
+endef 
+endif
+
+
+
diff --git a/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk
new file mode 100644
index 0000000..47dedfe
--- /dev/null
+++ b/60xx/libsensors_iio/software/build/android/shared.mk
@@ -0,0 +1,74 @@
+SHELL=/bin/bash
+
+TARGET       ?= android
+PRODUCT      ?= beagleboard
+ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair
+KERNEL_ROOT  ?= /Android/trunk/0xdroid/kernel
+MLSDK_ROOT   ?= $(CURDIR)
+
+ifeq ($(VERBOSE),1)
+	DUMP=1>/dev/stdout
+else
+	DUMP=1>/dev/null
+endif
+
+include common.mk
+
+################################################################################
+## targets
+
+INV_ROOT = ../..
+LIB_FOLDERS  = $(INV_ROOT)/core/mllite/build/$(TARGET)
+ifeq ($(BUILD_MPL),1)
+	LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
+endif
+APP_FOLDERS  = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
+
+INSTALL_DIR = $(CURDIR)
+
+################################################################################
+## macros
+
+define echo_in_colors
+	echo -ne "\e[1;34m"$(1)"\e[0m"
+endef
+
+################################################################################
+## rules
+
+.PHONY : all mllite mpl clean
+
+all:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+clean: 
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+cleanall:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
+install:
+	for DIR in $(LIB_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+	for DIR in $(APP_FOLDERS); do (				\
+		cd $$DIR && $(MAKE) -f shared.mk $@ ); 	\
+	done
+
diff --git a/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h
new file mode 100644
index 0000000..c519691
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/log.h
@@ -0,0 +1,364 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*
+ * 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 <stdlib.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
+#ifdef _WIN32
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+        __pragma (warning(suppress : 4127 )) \
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+            __pragma (warning(suppress : 4127 )) \
+    } while (0)
+#else
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+    } while (0)
+#endif
+
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#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
+#ifdef __KERNEL__
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((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);
+}
+
+#ifdef _WIN32
+/* The pragma removes warning about expression being constant */
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+        __pragma (warning(suppress : 4127 )) \
+	} while (0)
+#else
+#define LOG_RESULT_LOCATION(condition) \
+    do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+#endif
+
+
+#define INV_ERROR_CHECK(r_1329) \
+    if (r_1329) { \
+        LOG_RESULT_LOCATION(r_1329); \
+        return r_1329; \
+    }
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
new file mode 100644
index 0000000..9725199
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
@@ -0,0 +1,38 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef INV_INCLUDE_H__
+#define INV_INCLUDE_H__
+
+#define INVENSENSE_FUNC_START  typedef int invensensePutFunctionCallsHere
+
+#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/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
new file mode 100644
index 0000000..37194d6
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
@@ -0,0 +1,95 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $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/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
new file mode 100644
index 0000000..12f2901
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
@@ -0,0 +1,283 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef __MLSL_H__
+#define __MLSL_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ *  @defgroup   MLSL
+ *  @brief      Motion Library - Serial Layer.
+ *              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.
+ *
+ */
+
+/*
+ * 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 31
+
+#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.
+ */
+int 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.
+ */
+int inv_serial_close(void *sl_handle);
+
+/**
+ *  inv_serial_reset() - used to reset any buffering the driver may be doing
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int 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.
+ */
+int 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.
+ */
+int 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.
+ */
+int 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.
+ */
+int inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  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.
+ */
+int inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned char bank_reg,
+	unsigned char addr_reg,
+	unsigned char mem_reg,
+	unsigned short length,
+	unsigned char *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.
+ */
+int inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char fifo_reg,
+	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.
+ */
+int inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char fifo_reg,
+	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.
+ */
+int 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.
+ */
+int 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.
+ */
+int 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.
+ */
+int 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.
+ */
+int inv_serial_get_cal_length(unsigned int *len);
+#endif
+#ifdef __cplusplus
+}
+#endif
+/**
+ * @}
+ */
+#endif				/* __MLSL_H__ */
diff --git a/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
new file mode 100644
index 0000000..09eccce
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
@@ -0,0 +1,235 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Motion Library - Error definitions.
+ *          Definition of the error codes used within the MPL and
+ *          returned to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)               INV_SUCCESS
+ *          - (32)              INV_ERROR
+ *          - (22 / EINVAL)     INV_ERROR_INVALID_PARAMETER
+ *          - (1  / EPERM)      INV_ERROR_FEATURE_NOT_ENABLED
+ *          - (36)              INV_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (38)              INV_ERROR_DMP_NOT_STARTED
+ *          - (39)              INV_ERROR_DMP_STARTED
+ *          - (40)              INV_ERROR_NOT_OPENED
+ *          - (41)              INV_ERROR_OPENED
+ *          - (19 / ENODEV)     INV_ERROR_INVALID_MODULE
+ *          - (12 / ENOMEM)     INV_ERROR_MEMORY_EXAUSTED
+ *          - (44)              INV_ERROR_DIVIDE_BY_ZERO
+ *          - (45)              INV_ERROR_ASSERTION_FAILURE
+ *          - (46)              INV_ERROR_FILE_OPEN
+ *          - (47)              INV_ERROR_FILE_READ
+ *          - (48)              INV_ERROR_FILE_WRITE
+ *          - (49)              INV_ERROR_INVALID_CONFIGURATION
+ *          - (52)              INV_ERROR_SERIAL_CLOSED
+ *          - (53)              INV_ERROR_SERIAL_OPEN_ERROR
+ *          - (54)              INV_ERROR_SERIAL_READ
+ *          - (55)              INV_ERROR_SERIAL_WRITE
+ *          - (56)              INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (57)              INV_ERROR_SM_TRANSITION
+ *          - (58)              INV_ERROR_SM_IMPROPER_STATE
+ *          - (62)              INV_ERROR_FIFO_OVERFLOW
+ *          - (63)              INV_ERROR_FIFO_FOOTER
+ *          - (64)              INV_ERROR_FIFO_READ_COUNT
+ *          - (65)              INV_ERROR_FIFO_READ_DATA
+ *          - (72)              INV_ERROR_MEMORY_SET
+ *          - (82)              INV_ERROR_LOG_MEMORY_ERROR
+ *          - (83)              INV_ERROR_LOG_OUTPUT_ERROR
+ *          - (92)              INV_ERROR_OS_BAD_PTR
+ *          - (93)              INV_ERROR_OS_BAD_HANDLE
+ *          - (94)              INV_ERROR_OS_CREATE_FAILED
+ *          - (95)              INV_ERROR_OS_LOCK_FAILED
+ *          - (102)             INV_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (103)             INV_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (104)             INV_ERROR_COMPASS_DATA_NOT_READY
+ *          - (105)             INV_ERROR_COMPASS_DATA_ERROR
+ *          - (107)             INV_ERROR_CALIBRATION_LOAD
+ *          - (108)             INV_ERROR_CALIBRATION_STORE
+ *          - (109)             INV_ERROR_CALIBRATION_LEN
+ *          - (110)             INV_ERROR_CALIBRATION_CHECKSUM
+ *          - (111)             INV_ERROR_ACCEL_DATA_OVERFLOW
+ *          - (112)             INV_ERROR_ACCEL_DATA_UNDERFLOW
+ *          - (113)             INV_ERROR_ACCEL_DATA_NOT_READY
+ *          - (114)             INV_ERROR_ACCEL_DATA_ERROR
+ *
+ *          The available warning codes and their associated values are:
+ *          - (115)             INV_WARNING_MOTION_RACE
+ *          - (116)             INV_WARNING_QUAT_TRASHED
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#include <asm-generic/errno-base.h>
+#else
+#include "stdint_invensense.h"
+#include <errno.h>
+#endif
+#include <limits.h>
+
+#ifndef REMOVE_INV_ERROR_T
+/*---------------------------
+ *    ML Types
+ *--------------------------*/
+
+/**
+ *  @struct inv_error_t mltypes.h "mltypes"
+ *  @brief  The MPL Error Code return type.
+ *
+ *  @code
+ *      typedef unsigned char inv_error_t;
+ *  @endcode
+ */
+//typedef unsigned char inv_error_t;
+typedef int inv_error_t;
+#endif
+
+typedef long long inv_time_t;
+
+#if !defined __GNUC__ && !defined __KERNEL__
+typedef int8_t   __s8;
+typedef int16_t  __s16;
+typedef int32_t  __s32;
+typedef int32_t  __s64;
+
+typedef uint8_t   __u8;
+typedef uint16_t  __u16;
+typedef uint32_t  __u32;
+typedef uint64_t  __u64;
+#elif !defined __KERNEL__
+#include <sys/types.h>
+#endif
+
+#ifndef __cplusplus
+#ifndef __KERNEL__
+typedef int_fast8_t bool;
+
+#ifndef false
+#define false 0
+#endif
+
+#ifndef true
+#define true 1
+#endif
+
+#endif
+#endif
+
+/*---------------------------
+ *    ML Defines
+ *--------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifndef __KERNEL__
+#ifndef ARRAY_SIZE
+/* Dimension of an array */
+#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
+#endif
+#endif
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK_FIRST(first, x) \
+	{ if (INV_SUCCESS == first) first = x; }
+
+#define INV_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define INV_ERROR_BASE                    (0x20)
+#define INV_ERROR                         (INV_ERROR_BASE)
+
+/* Compatibility and other generic error codes */
+#define INV_ERROR_INVALID_PARAMETER             (EINVAL)
+#define INV_ERROR_FEATURE_NOT_ENABLED           (EPERM)
+#define INV_ERROR_FEATURE_NOT_IMPLEMENTED       (INV_ERROR_BASE + 4)
+#define INV_ERROR_DMP_NOT_STARTED               (INV_ERROR_BASE + 6)
+#define INV_ERROR_DMP_STARTED                   (INV_ERROR_BASE + 7)
+#define INV_ERROR_NOT_OPENED                    (INV_ERROR_BASE + 8)
+#define INV_ERROR_OPENED                        (INV_ERROR_BASE + 9)
+#define INV_ERROR_INVALID_MODULE                (ENODEV)
+#define INV_ERROR_MEMORY_EXAUSTED               (ENOMEM)
+#define INV_ERROR_DIVIDE_BY_ZERO                (INV_ERROR_BASE + 12)
+#define INV_ERROR_ASSERTION_FAILURE             (INV_ERROR_BASE + 13)
+#define INV_ERROR_FILE_OPEN                     (INV_ERROR_BASE + 14)
+#define INV_ERROR_FILE_READ                     (INV_ERROR_BASE + 15)
+#define INV_ERROR_FILE_WRITE                    (INV_ERROR_BASE + 16)
+#define INV_ERROR_INVALID_CONFIGURATION         (INV_ERROR_BASE + 17)
+#define INV_ERROR_NOT_AUTHORIZED                (INV_ERROR_BASE + 18)
+
+/* Serial Communication */
+#define INV_ERROR_SERIAL_CLOSED                 (INV_ERROR_BASE + 20)
+#define INV_ERROR_SERIAL_OPEN_ERROR             (INV_ERROR_BASE + 21)
+#define INV_ERROR_SERIAL_READ                   (INV_ERROR_BASE + 22)
+#define INV_ERROR_SERIAL_WRITE                  (INV_ERROR_BASE + 23)
+#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (INV_ERROR_BASE + 24)
+
+/* SM = State Machine */
+#define INV_ERROR_SM_TRANSITION                 (INV_ERROR_BASE + 25)
+#define INV_ERROR_SM_IMPROPER_STATE             (INV_ERROR_BASE + 26)
+
+/* Fifo */
+#define INV_ERROR_FIFO_OVERFLOW                 (INV_ERROR_BASE + 30)
+#define INV_ERROR_FIFO_FOOTER                   (INV_ERROR_BASE + 31)
+#define INV_ERROR_FIFO_READ_COUNT               (INV_ERROR_BASE + 32)
+#define INV_ERROR_FIFO_READ_DATA                (INV_ERROR_BASE + 33)
+
+/* Memory & Registers, Set & Get */
+#define INV_ERROR_MEMORY_SET                    (INV_ERROR_BASE + 40)
+
+#define INV_ERROR_LOG_MEMORY_ERROR              (INV_ERROR_BASE + 50)
+#define INV_ERROR_LOG_OUTPUT_ERROR              (INV_ERROR_BASE + 51)
+
+/* OS interface errors */
+#define INV_ERROR_OS_BAD_PTR                    (INV_ERROR_BASE + 60)
+#define INV_ERROR_OS_BAD_HANDLE                 (INV_ERROR_BASE + 61)
+#define INV_ERROR_OS_CREATE_FAILED              (INV_ERROR_BASE + 62)
+#define INV_ERROR_OS_LOCK_FAILED                (INV_ERROR_BASE + 63)
+
+/* Compass errors */
+#define INV_ERROR_COMPASS_DATA_OVERFLOW         (INV_ERROR_BASE + 70)
+#define INV_ERROR_COMPASS_DATA_UNDERFLOW        (INV_ERROR_BASE + 71)
+#define INV_ERROR_COMPASS_DATA_NOT_READY        (INV_ERROR_BASE + 72)
+#define INV_ERROR_COMPASS_DATA_ERROR            (INV_ERROR_BASE + 73)
+
+/* Load/Store calibration */
+#define INV_ERROR_CALIBRATION_LOAD              (INV_ERROR_BASE + 75)
+#define INV_ERROR_CALIBRATION_STORE             (INV_ERROR_BASE + 76)
+#define INV_ERROR_CALIBRATION_LEN               (INV_ERROR_BASE + 77)
+#define INV_ERROR_CALIBRATION_CHECKSUM          (INV_ERROR_BASE + 78)
+
+/* Accel errors */
+#define INV_ERROR_ACCEL_DATA_OVERFLOW           (INV_ERROR_BASE + 79)
+#define INV_ERROR_ACCEL_DATA_UNDERFLOW          (INV_ERROR_BASE + 80)
+#define INV_ERROR_ACCEL_DATA_NOT_READY          (INV_ERROR_BASE + 81)
+#define INV_ERROR_ACCEL_DATA_ERROR              (INV_ERROR_BASE + 82)
+
+/* No Motion Warning States */
+#define INV_WARNING_MOTION_RACE                 (INV_ERROR_BASE + 83)
+#define INV_WARNING_QUAT_TRASHED                (INV_ERROR_BASE + 84)
+#define INV_WARNING_GYRO_MAG                    (INV_ERROR_BASE + 85)
+
+#define INV_WARNING_SEMAPHORE_TIMEOUT           (INV_ERROR_BASE + 86)
+
+
+/* For Linux coding compliance */
+#ifndef __KERNEL__
+#define EXPORT_SYMBOL(x)
+#endif
+
+#endif				/* MLTYPES_H */
diff --git a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
new file mode 100644
index 0000000..b8c2511
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
@@ -0,0 +1,41 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef STDINT_INVENSENSE_H
+#define STDINT_INVENSENSE_H
+
+#ifndef WIN32
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#endif
+
+#else
+
+#include <windows.h>
+
+typedef signed char int8_t;
+typedef short int16_t;
+typedef long int32_t;
+typedef long long int64_t;
+
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned long uint32_t;
+typedef unsigned long long uint64_t;
+
+typedef int int_fast8_t;
+typedef int int_fast16_t;
+typedef long int_fast32_t;
+
+typedef unsigned int uint_fast8_t;
+typedef unsigned int uint_fast16_t;
+typedef unsigned long uint_fast32_t;
+
+#endif
+
+#endif // STDINT_INVENSENSE_H
diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
new file mode 100755
index 0000000..9bdd5bc
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
new file mode 100644
index 0000000..1418450
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -0,0 +1,87 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc 
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT = ../../../../..
+MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(INV_ROOT)/simple_apps/common
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc 
+LLINK += -lm 
+LLINK += -lutils 
+LLINK += -lcutils 
+LLINK += -lgcc 
+LLINK += -ldl
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -shared 
+LFLAGS += -Wl,-soname,$(LIBRARY)
+LFLAGS += -Wl,-shared,-Bsymbolic
+LFLAGS += $(ANDROID_LINK)
+LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+#INV_SOURCES provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all mllite clean cleanall makefiles
+
+all: mllite
+
+mllite: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
+	$(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
new file mode 100644
index 0000000..011120c
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
@@ -0,0 +1,42 @@
+#### filelist.mk for mllite ####
+
+# headers only
+HEADERS := $(MLLITE_DIR)/invensense.h
+
+# headers for sources
+HEADERS += $(MLLITE_DIR)/data_builder.h
+HEADERS += $(MLLITE_DIR)/hal_outputs.h
+HEADERS += $(MLLITE_DIR)/message_layer.h
+HEADERS += $(MLLITE_DIR)/ml_math_func.h
+HEADERS += $(MLLITE_DIR)/mpl.h
+HEADERS += $(MLLITE_DIR)/results_holder.h
+HEADERS += $(MLLITE_DIR)/start_manager.h
+HEADERS += $(MLLITE_DIR)/storage_manager.h
+
+# headers (linux specific)
+HEADERS += $(MLLITE_DIR)/linux/mlos.h
+HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h
+HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h
+HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h
+
+# sources
+SOURCES := $(MLLITE_DIR)/data_builder.c
+SOURCES += $(MLLITE_DIR)/hal_outputs.c
+SOURCES += $(MLLITE_DIR)/message_layer.c
+SOURCES += $(MLLITE_DIR)/ml_math_func.c
+SOURCES += $(MLLITE_DIR)/mpl.c
+SOURCES += $(MLLITE_DIR)/results_holder.c
+SOURCES += $(MLLITE_DIR)/start_manager.c
+SOURCES += $(MLLITE_DIR)/storage_manager.c
+
+# sources (linux specific)
+SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c
+SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c
+SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c
+SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c
+
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux
+
diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c
new file mode 100644
index 0000000..0aa418d
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c
@@ -0,0 +1,1228 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+ 
+/**
+ *   @defgroup  Data_Builder data_builder
+ *   @brief     Motion Library - Data Builder
+ *              Constructs and Creates the data for MPL
+ *
+ *   @{
+ *       @file data_builder.c
+ *       @brief Data Builder.
+ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
+
+#include <string.h>
+
+#include "ml_math_func.h"
+#include "data_builder.h"
+#include "mlmath.h"
+#include "storage_manager.h"
+#include "message_layer.h"
+#include "results_holder.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL"
+
+typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data);
+
+struct process_t {
+    inv_process_cb_func func;
+    int priority;
+    int data_required;
+};
+
+struct inv_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+    /** Sensor Accuracy */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
+};
+
+struct inv_data_builder_t {
+    int num_cb;
+    struct process_t process[INV_MAX_DATA_CB];
+    struct inv_db_save_t save;
+    int compass_disturbance;
+    int mode;
+#ifdef INV_PLAYBACK_DBG
+    int debug_mode;
+    int last_mode;
+    FILE *file;
+#endif
+};
+
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias);
+static void inv_set_contiguous(void);
+
+static struct inv_data_builder_t inv_data_builder;
+static struct inv_sensor_cal_t sensors;
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53395
+
+#ifdef INV_PLAYBACK_DBG
+
+/** Turn on data logging to allow playback of same scenario at a later time.
+* @param[in] file File to write to, must be open.
+*/
+void inv_turn_on_data_logging(FILE *file)
+{
+    MPL_LOGV("input data logging started\n");
+    inv_data_builder.file = file;
+    inv_data_builder.debug_mode = RD_RECORD;
+}
+
+/** Turn off data logging to allow playback of same scenario at a later time.
+* File passed to inv_turn_on_data_logging() must be closed after calling this.
+*/
+void inv_turn_off_data_logging()
+{
+    MPL_LOGV("input data logging stopped\n");
+    inv_data_builder.debug_mode = RD_NO_DEBUG;
+    inv_data_builder.file = NULL;
+}
+#endif
+
+/** This function receives the data that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+    // copy in the saved accuracy in the actual sensors accuracy
+    sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
+    sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
+    sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
+    // TODO
+    if (sensors.compass.accuracy == 3) {
+        inv_set_compass_bias_found(1);
+    }
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save));
+    return INV_SUCCESS;
+}
+
+/** Initialize the data builder
+*/
+inv_error_t inv_init_data_builder(void)
+{
+    /* TODO: Hardcode temperature scale/offset here. */
+    memset(&inv_data_builder, 0, sizeof(inv_data_builder));
+    memset(&sensors, 0, sizeof(sensors));
+    return inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                   sizeof(inv_data_builder.save),
+                                   INV_DB_SAVE_KEY);
+}
+
+/** Gyro sensitivity.
+* @return A scale factor to convert device units to degrees per second scaled by 2^16
+* such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum rate * 2^15.
+*/
+long inv_get_gyro_sensitivity()
+{
+    return sensors.gyro.sensitivity;
+}
+
+/** Accel sensitivity.
+* @return A scale factor to convert device units to g's scaled by 2^16
+* such that g_s  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum accel value in g's * 2^15.
+*/
+long inv_get_accel_sensitivity(void)
+{
+    return sensors.accel.sensitivity;
+}
+
+/** Compass sensitivity.
+* @return A scale factor to convert device units to micro Tesla scaled by 2^16
+* such that uT  = device_units * sensitivity / 2^30. Typically
+* it works out to be the maximum uT * 2^15.
+*/
+long inv_get_compass_sensitivity(void)
+{
+    return sensors.compass.sensitivity;
+}
+
+/** Sets orientation and sensitivity field for a sensor.
+* @param[out] sensor Structure to apply settings to
+* @param[in] orientation Orientation description of how part is mounted.
+* @param[in] sensitivity A Scale factor to convert from hardware units to
+*            standard units (dps, uT, g).
+*/
+void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
+                                 int orientation, long sensitivity)
+{
+    sensor->sensitivity = sensitivity;
+    sensor->orientation = orientation;
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16
+*            such that degrees_per_second  = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum rate * 2^15.
+*/
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.gyro, orientation,
+                                     sensitivity);
+}
+
+/** Set Gyro Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in us
+*/
+void inv_set_gyro_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.gyro.sample_rate_us = sample_rate_us;
+    sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.gyro.bandwidth == 0) {
+        sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Set Accel Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Accel Sample rate in us
+*/
+void inv_set_accel_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.accel.sample_rate_us = sample_rate_us;
+    sensors.accel.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.accel.bandwidth == 0) {
+        sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+/** Pick the smallest non-negative number. Priority to td1 on equal
+* If both are negative, return the largest.
+*/
+static int inv_pick_best_time_difference(long td1, long td2)
+{
+    if (td1 >= 0) {
+        if (td2 >= 0) {
+            if (td1 <= td2) {
+                // td1
+                return 0;
+            } else {
+                // td2
+                return 1;
+            }
+        } else {
+            // td1
+            return 0;
+        }
+    } else if (td2 >= 0) {
+        // td2
+        return 1;
+    } else {
+        // Both are negative
+        if (td1 >= td2) {
+            // td1
+            return 0;
+        } else {
+            // td2
+            return 1;
+        }
+    }
+}
+
+/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data.
+*/
+static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts)
+{
+    int status = 0;
+    switch (sensor_number) {
+    case 0: // Quat
+        *ts = sensors.quat.timestamp;
+        if (inv_data_builder.mode & INV_QUAT_NEW)
+            if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
+                status = 1;
+        return status;
+    case 1: // Gyro
+        *ts = sensors.gyro.timestamp;
+        if (inv_data_builder.mode & INV_GYRO_NEW)
+            if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp)
+                status = 1;
+        return status;
+    case 2: // Accel
+        *ts = sensors.accel.timestamp;
+        if (inv_data_builder.mode & INV_ACCEL_NEW)
+            if (sensors.accel.timestamp_prev != sensors.accel.timestamp)
+                status = 1;
+        return status;
+   case 3: // Compass
+        *ts = sensors.compass.timestamp;
+        if (inv_data_builder.mode & INV_MAG_NEW)
+            if (sensors.compass.timestamp_prev != sensors.compass.timestamp)
+                status = 1;
+        return status;
+    default:
+        *ts = 0;
+        return 0;
+    }
+    return 0;
+}
+
+/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination.
+* It does this by finding a raw sensor that has the closest sample rate that is at least as
+* often desired. It also returns if that raw sensor has a new piece of data.
+* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel
+* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise.
+*/
+int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts)
+{
+    long td[2];
+    int idx;
+
+    if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
+        // Sensor number is 0 (Quat)
+        return inv_raw_sensor_timestamp(0, ts);
+    } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
+        return 0; // Accel must be on or 6-axis quat must be on
+    }
+
+    // At this point, we know accel is on. Check if 3-axis quat is on
+    td[0] = sample_rate_us - sensors.quat.sample_rate_us;
+    td[1] = sample_rate_us - sensors.accel.sample_rate_us;
+    if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
+        idx = inv_pick_best_time_difference(td[0], td[1]);
+        idx *= 2;
+        // 0 = quat, 3=accel
+        return inv_raw_sensor_timestamp(idx, ts);
+    }
+
+    // No Quaternion data from outside, Gyro must be on
+    if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
+        return 0; // Gyro must be on
+    }
+
+    td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
+    idx = inv_pick_best_time_difference(td[0], td[1]);
+    idx *= 2; // 0=gyro 2=accel
+    idx++;
+    // 1 = gyro, 3=accel
+    return inv_raw_sensor_timestamp(idx, ts);
+}
+
+/** Set Compass Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds.
+*/
+void inv_set_compass_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.compass.sample_rate_us = sample_rate_us;
+    sensors.compass.sample_rate_ms = sample_rate_us / 1000;
+    if (sensors.compass.bandwidth == 0) {
+        sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
+    }
+}
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.gyro.sample_rate_ms;
+}
+
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.accel.sample_rate_ms;
+}
+
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.compass.sample_rate_ms;
+}
+
+/** Set Quat Sample rate in micro seconds.
+* @param[in] sample_rate_us Set Quat Sample rate in us
+*/
+void inv_set_quat_sample_rate(long sample_rate_us)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.quat.sample_rate_us = sample_rate_us;
+    sensors.quat.sample_rate_ms = sample_rate_us / 1000;
+}
+
+/** Set Gyro Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_gyro_bandwidth(int bandwidth_hz)
+{
+    sensors.gyro.bandwidth = bandwidth_hz;
+}
+
+/** Set Accel Bandwidth in Hz
+* @param[in] bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_accel_bandwidth(int bandwidth_hz)
+{
+    sensors.accel.bandwidth = bandwidth_hz;
+}
+
+/** Set Compass Bandwidth in Hz
+* @param[in]  bandwidth_hz Gyro bandwidth in Hz
+*/
+void inv_set_compass_bandwidth(int bandwidth_hz)
+{
+    sensors.compass.bandwidth = bandwidth_hz;
+}
+
+/** Helper function stating whether the compass is on or off.
+ * @return TRUE if compass if on, 0 if compass if off
+*/
+int inv_get_compass_on()
+{
+    return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the gyro is on or off.
+ * @return TRUE if gyro if on, 0 if gyro if off
+*/
+int inv_get_gyro_on()
+{
+    return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Helper function stating whether the acceleromter is on or off.
+ * @return TRUE if accel if on, 0 if accel if off
+*/
+int inv_get_accel_on()
+{
+    return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
+}
+
+/** Get last timestamp across all 3 sensors that are on.
+* This find out which timestamp has the largest value for sensors that are on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_time_t inv_get_last_timestamp()
+{
+    inv_time_t timestamp = 0;
+    if (sensors.accel.status & INV_SENSOR_ON) {
+        timestamp = sensors.accel.timestamp;
+    }
+    if (sensors.gyro.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.gyro.timestamp) {
+            timestamp = sensors.gyro.timestamp;
+        }
+    }
+    if (sensors.compass.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.compass.timestamp) {
+            timestamp = sensors.compass.timestamp;
+        }
+    }
+    if (sensors.temp.status & INV_SENSOR_ON) {
+        if (timestamp < sensors.temp.timestamp)
+            timestamp = sensors.temp.timestamp;
+    }
+    return timestamp;
+}
+
+/** Sets the orientation and sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to g's
+*            such that g's = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum g_value * 2^15.
+*/
+void inv_set_accel_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_A_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.accel, orientation,
+                                     sensitivity);
+}
+
+/** Sets the Orientation and Sensitivity of the gyro data.
+* @param[in] orientation A scalar defining the transformation from chip mounting
+*            to the body frame. The function inv_orientation_matrix_to_scalar()
+*            can convert the transformation matrix to this scalar and describes the
+*            scalar in further detail.
+* @param[in] sensitivity A scale factor to convert device units to uT
+*            such that uT = device_units * sensitivity / 2^30. Typically
+*            it works out to be the maximum uT_value * 2^15.
+*/
+void inv_set_compass_orientation_and_scale(int orientation, long sensitivity)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_C_ORIENT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
+        fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
+    }
+#endif
+    set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
+}
+
+void inv_matrix_vector_mult(const long *A, const long *x, long *y)
+{
+    y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
+    y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
+    y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
+}
+
+/** Takes raw data stored in the sensor, removes bias, and converts it to
+* calibrated data in the body frame. Also store raw data for body frame.
+* @param[in,out] sensor structure to modify
+* @param[in] bias bias in the mounting frame, in hardware units scaled by
+*                 2^16. Length 3.
+*/
+void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
+{
+    long raw32[3];
+
+    // Convert raw to calibrated
+    raw32[0] = (long)sensor->raw[0] << 15;
+    raw32[1] = (long)sensor->raw[1] << 15;
+    raw32[2] = (long)sensor->raw[2] << 15;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
+
+    raw32[0] -= bias[0] >> 1;
+    raw32[1] -= bias[1] >> 1;
+    raw32[2] -= bias[2] >> 1;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
+
+    sensor->status |= INV_CALIBRATED;
+}
+
+/** Returns the current bias for the compass
+* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame.
+*             Length 3.
+*/
+void inv_get_compass_bias(long *bias)
+{
+    if (bias != NULL) {
+        memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias));
+    }
+}
+
+void inv_set_compass_bias(const long *bias, int accuracy)
+{
+    if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) {
+        memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias));
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+    }
+    sensors.compass.accuracy = accuracy;
+    inv_data_builder.save.compass_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
+}
+
+/** Set the state of a compass disturbance
+* @param[in] dist 1=disturbance, 0=no disturbance
+*/
+void inv_set_compass_disturbance(int dist)
+{
+    inv_data_builder.compass_disturbance = dist;
+}
+
+int inv_get_compass_disturbance(void) {
+    return inv_data_builder.compass_disturbance;
+}
+/** Sets the accel bias.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_bias(const long *bias, int accuracy)
+{
+    if (bias) {
+        if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) {
+            memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias));
+            inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+        }
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel accuracy.
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_accuracy(int accuracy)
+{
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel bias with control over which axis.
+* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+* @param[in] mask Mask to select axis to apply bias set.
+*/
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
+{
+    if (bias) {
+        if (mask & 1){
+            inv_data_builder.save.accel_bias[0] = bias[0];
+        }
+        if (mask & 2){
+            inv_data_builder.save.accel_bias[1] = bias[1];
+        }
+        if (mask & 4){
+            inv_data_builder.save.accel_bias[2] = bias[2];
+        }
+
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    }
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+
+/** Sets the gyro bias
+* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame.
+*            Length 3.
+* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate.
+*/
+void inv_set_gyro_bias(const long *bias, int accuracy)
+{
+    if (bias != NULL) {
+        if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) {
+            memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias));
+            inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
+        }
+    }
+    sensors.gyro.accuracy = accuracy;
+    inv_data_builder.save.gyro_accuracy = accuracy;
+
+    /* TODO: What should we do if there's no temperature data? */
+    if (sensors.temp.calibrated[0])
+        inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
+    else
+        /* Set to 27 deg C for now until we've got a better solution. */
+        inv_data_builder.save.gyro_temp = 1769472L;
+    inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
+}
+
+/* TODO: Add this information to inv_sensor_cal_t */
+/** 
+ *  Get the gyro biases and temperature record from MPL 
+ *  @param[in] bias 
+ *              Gyro bias in hardware units scaled by 2^16. 
+ *              In chip mounting frame.
+ *              Length 3.
+ *  @param[in] temp
+ *              Tempearature in degrees C. 
+ */
+void inv_get_gyro_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.gyro_bias,
+               sizeof(inv_data_builder.save.gyro_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.gyro_temp;
+}
+
+/** Get Accel Bias
+* @param[out] bias Accel bias where
+* @param[out] temp Temperature where 1 C = 2^16
+*/
+void inv_get_accel_bias(long *bias, long *temp)
+{
+    if (bias != NULL)
+        memcpy(bias, inv_data_builder.save.accel_bias,
+               sizeof(inv_data_builder.save.accel_bias));
+    if (temp != NULL)
+        temp[0] = inv_data_builder.save.accel_temp;
+}
+
+/** 
+ *  Record new accel data for use when inv_execute_on_data() is called
+ *  @param[in]  accel accel data. 
+ *              Length 3. 
+ *              Calibrated data is in m/s^2 scaled by 2^16 in body frame. 
+ *              Raw data is in device units in chip mounting frame.
+ *  @param[in]  status 
+ *              Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 
+ *              being most accurate.
+ *              The upper bit INV_CALIBRATED, is set if the data was calibrated 
+ *              outside MPL and it is not set if the data being passed is raw. 
+ *              Raw data should be in device units, typically in a 16-bit range.
+ *  @param[in]  timestamp 
+ *              Monotonic time stamp, for Android it's in nanoseconds.
+ *  @return     Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_ACCEL;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.accel.raw[0] = (short)accel[0];
+        sensors.accel.raw[1] = (short)accel[1];
+        sensors.accel.raw[2] = (short)accel[2];
+        sensors.accel.status |= INV_RAW_DATA;
+        inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
+    } else {
+        sensors.accel.calibrated[0] = accel[0];
+        sensors.accel.calibrated[1] = accel[1];
+        sensors.accel.calibrated[2] = accel[2];
+        sensors.accel.status |= INV_CALIBRATED;
+        sensors.accel.accuracy = status & 3;
+        inv_data_builder.save.accel_accuracy = status & 3;
+    }
+    sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
+    sensors.accel.timestamp_prev = sensors.accel.timestamp;
+    sensors.accel.timestamp = timestamp;
+
+    return INV_SUCCESS;
+}
+
+/** Record new gyro data and calls inv_execute_on_data() if previous
+* sample has not been processed.
+* @param[in] gyro Data is in device units. Length 3.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_GYRO;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
+    sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
+    sensors.gyro.timestamp = timestamp;
+    inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
+
+    return INV_SUCCESS;
+}
+
+/** Record new compass data for use when inv_execute_on_data() is called
+* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16.
+*            Length 3.
+* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate.
+*            The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is
+*            not set if the data being passed is raw. Raw data should be in device units, typically
+*            in a 16-bit range.
+* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_compass(const long *compass, int status,
+                              inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_COMPASS;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+
+    if ((status & INV_CALIBRATED) == 0) {
+        sensors.compass.raw[0] = (short)compass[0];
+        sensors.compass.raw[1] = (short)compass[1];
+        sensors.compass.raw[2] = (short)compass[2];
+        inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
+        sensors.compass.status |= INV_RAW_DATA;
+    } else {
+        sensors.compass.calibrated[0] = compass[0];
+        sensors.compass.calibrated[1] = compass[1];
+        sensors.compass.calibrated[2] = compass[2];
+        sensors.compass.status |= INV_CALIBRATED;
+        sensors.compass.accuracy = status & 3;
+        inv_data_builder.save.compass_accuracy = status & 3;
+    }
+    sensors.compass.timestamp_prev = sensors.compass.timestamp;
+    sensors.compass.timestamp = timestamp;
+    sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
+
+    return INV_SUCCESS;
+}
+
+/** Record new temperature data for use when inv_execute_on_data() is called.
+ *  @param[in]  temp Temperature data in q16 format.
+ *  @param[in]  timestamp   Monotonic time stamp; for Android it's in
+ *                          nanoseconds.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_TEMPERATURE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(&temp, sizeof(temp), 1, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    sensors.temp.calibrated[0] = temp;
+    sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.temp.timestamp_prev = sensors.temp.timestamp;
+    sensors.temp.timestamp = timestamp;
+    /* TODO: Apply scale, remove offset. */
+
+    return INV_SUCCESS;
+}
+/** quaternion data
+* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. 
+*                 Real part first. Length 4.  
+* @param[in] status number of axis, 16-bit or 32-bit
+* @param[in] timestamp
+* @param[in]  timestamp   Monotonic time stamp; for Android it's in
+*                         nanoseconds.
+* @param[out] executed Set to 1 if data processing was done.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp)
+{
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_QUAT;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+        fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file);
+        fwrite(&timestamp, sizeof(timestamp), 1, inv_data_builder.file);
+    }
+#endif
+    
+    memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
+    sensors.quat.timestamp = timestamp;
+    sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
+    sensors.quat.status |= (INV_BIAS_APPLIED & status);
+
+    return INV_SUCCESS;
+}
+
+/** This should be called when the accel has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_accel_was_turned_off()
+{
+    sensors.accel.status = 0;
+}
+
+/** This should be called when the compass has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_compass_was_turned_off()
+{
+    sensors.compass.status = 0;
+}
+
+/** This should be called when the quaternion data from the DMP has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_quaternion_sensor_was_turned_off(void)
+{
+    sensors.quat.status = 0;
+}
+
+/** This should be called when the gyro has been turned off. This is so
+* that we will know if the data is contiguous.
+*/
+void inv_gyro_was_turned_off()
+{
+    sensors.gyro.status = 0;
+}
+
+/** This should be called when the temperature sensor has been turned off.
+ *  This is so that we will know if the data is contiguous.
+ */
+void inv_temperature_was_turned_off()
+{
+    sensors.temp.status = 0;
+}
+
+/** Registers to receive a callback when there is new sensor data.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_register_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data),
+    int priority, int sensor_type)
+{
+    inv_error_t result = INV_SUCCESS;
+    int kk, nn;
+
+    // Make sure we haven't registered this function already
+    // Or used the same priority
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if ((inv_data_builder.process[kk].func == func) ||
+                (inv_data_builder.process[kk].priority == priority)) {
+            return INV_ERROR_INVALID_PARAMETER;    //fixme give a warning
+        }
+    }
+
+    // Make sure we have not filled up our number of allowable callbacks
+    if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
+        kk = 0;
+        if (inv_data_builder.num_cb != 0) {
+            // set kk to be where this new callback goes in the array
+            while ((kk < inv_data_builder.num_cb) &&
+                    (inv_data_builder.process[kk].priority < priority)) {
+                kk++;
+            }
+            if (kk != inv_data_builder.num_cb) {
+                // We need to move the others
+                for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
+                    inv_data_builder.process[nn] =
+                        inv_data_builder.process[nn - 1];
+                }
+            }
+        }
+        // Add new callback
+        inv_data_builder.process[kk].func = func;
+        inv_data_builder.process[kk].priority = priority;
+        inv_data_builder.process[kk].data_required = sensor_type;
+        inv_data_builder.num_cb++;
+    } else {
+        MPL_LOGE("Unable to add feature callback as too many were already registered\n");
+        result = INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    return result;
+}
+
+/** Unregisters the callback that happens when new sensor data is received.
+* @internal
+* @param[in] func Function pointer to receive callback when there is new sensor data
+* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
+*            numbers must be unique.
+* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
+*            a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
+*            gyro data, INV_MAG_NEW = compass data. So passing in
+*            INV_ACCEL_NEW | INV_MAG_NEW, a
+*            callback would be generated if there was new magnetomer data OR new accel data.
+*/
+inv_error_t inv_unregister_data_cb(
+    inv_error_t (*func)(struct inv_sensor_cal_t *data))
+{
+    int kk, nn;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.process[kk].func == func) {
+            // Delete this callback
+            for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) {
+                inv_data_builder.process[nn - 1] =
+                    inv_data_builder.process[nn];
+            }
+            inv_data_builder.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+
+    return INV_SUCCESS;    // We did not find the callback
+}
+
+/** After at least one of inv_build_gyro(), inv_build_accel(), or
+* inv_build_compass() has been called, this function should be called.
+* It will process the data it has received and update all the internal states
+* and features that have been turned on.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_on_data(void)
+{
+    inv_error_t result, first_error;
+    int kk;
+
+#ifdef INV_PLAYBACK_DBG
+    if (inv_data_builder.debug_mode == RD_RECORD) {
+        int type = PLAYBACK_DBG_TYPE_EXECUTE;
+        fwrite(&type, sizeof(type), 1, inv_data_builder.file);
+    }
+#endif
+    // Determine what new data we have
+    inv_data_builder.mode = 0;
+    if (sensors.gyro.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_GYRO_NEW;
+    if (sensors.accel.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_ACCEL_NEW;
+    if (sensors.compass.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_MAG_NEW;
+    if (sensors.temp.status & INV_NEW_DATA)
+        inv_data_builder.mode |= INV_TEMP_NEW;
+    if (sensors.quat.status & INV_QUAT_NEW)
+        inv_data_builder.mode |= INV_QUAT_NEW;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
+        if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) {
+            result = inv_data_builder.process[kk].func(&sensors);
+            if (result && !first_error) {
+                first_error = result;
+            }
+        }
+    }
+
+    inv_set_contiguous();
+
+    return first_error;
+}
+
+/** Cleans up status bits after running all the callbacks. It sets the contiguous flag.
+*
+*/
+static void inv_set_contiguous(void)
+{
+    inv_time_t current_time = 0;
+    if (sensors.gyro.status & INV_NEW_DATA) {
+        sensors.gyro.status |= INV_CONTIGUOUS;
+        current_time = sensors.gyro.timestamp;
+    }
+    if (sensors.accel.status & INV_NEW_DATA) {
+        sensors.accel.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.accel.timestamp);
+    }
+    if (sensors.compass.status & INV_NEW_DATA) {
+        sensors.compass.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.compass.timestamp);
+    }
+    if (sensors.temp.status & INV_NEW_DATA) {
+        sensors.temp.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.temp.timestamp);
+    }
+    if (sensors.quat.status & INV_NEW_DATA) {
+        sensors.quat.status |= INV_CONTIGUOUS;
+        current_time = MAX(current_time, sensors.quat.timestamp);
+    }
+
+#if 0
+    /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
+     * type functions. This is just in case that breaks down. We make sure
+     * all the data is within 2 seconds of the newest piece of data*/
+    if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
+        inv_gyro_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
+        inv_accel_was_turned_off();
+    if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
+        inv_compass_was_turned_off();
+    /* TODO: Temperature might not need to be read this quickly. */
+    if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
+        inv_temperature_was_turned_off();
+#endif
+
+    /* clear bits */
+    sensors.gyro.status &= ~INV_NEW_DATA;
+    sensors.accel.status &= ~INV_NEW_DATA;
+    sensors.compass.status &= ~INV_NEW_DATA;
+    sensors.temp.status &= ~INV_NEW_DATA;
+    sensors.quat.status &= ~INV_NEW_DATA;
+}
+
+/** Gets a whole set of accel data including data, accuracy and timestamp.
+ * @param[out] data Accel Data where 1g = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    if (data != NULL) {
+        memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
+    }
+    if (timestamp != NULL) {
+        *timestamp = sensors.accel.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.accel.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.gyro.accuracy;
+    }
+}
+
+/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.gyro.accuracy;
+    }
+}
+
+/** Get's latest gyro data.
+* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
+*/
+void inv_get_gyro(long *gyro)
+{
+    memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
+}
+
+/** Gets a whole set of compass data including data, accuracy and timestamp.
+ * @param[out] data Compass Data where 1 uT = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
+    if (timestamp != NULL) {
+        *timestamp = sensors.compass.timestamp;
+    }
+    if (accuracy != NULL) {
+        if (inv_data_builder.compass_disturbance)
+            *accuracy = 0;
+        else
+            *accuracy = sensors.compass.accuracy;
+    }
+}
+
+/** Gets a whole set of temperature data including data, accuracy and timestamp.
+ *  @param[out] data        Temperature data where 1 degree C = 2^16
+ *  @param[out] accuracy    0 to 3, where 3 is most accurate.
+ *  @param[out] timestamp   The timestamp of the data sample.
+ */
+void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+    data[0] = sensors.temp.calibrated[0];
+    if (timestamp)
+        *timestamp = sensors.temp.timestamp;
+    if (accuracy)
+        *accuracy = sensors.temp.accuracy;
+}
+
+/** Returns accuracy of gyro.
+ * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_gyro_accuracy(void)
+{
+    return sensors.gyro.accuracy;
+}
+
+/** Returns accuracy of compass.
+ * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_mag_accuracy(void)
+{
+    if (inv_data_builder.compass_disturbance)
+        return 0;
+    return sensors.compass.accuracy;
+}
+
+/** Returns accuracy of accel.
+ * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate.
+*/
+int inv_get_accel_accuracy(void)
+{
+    return sensors.accel.accuracy;
+}
+
+inv_error_t inv_get_gyro_orient(int *orient)
+{
+    *orient = sensors.gyro.orientation;
+    return 0;
+}
+
+inv_error_t inv_get_accel_orient(int *orient)
+{
+    *orient = sensors.accel.orientation;
+    return 0;
+}
+
+
+/**
+ * @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h
new file mode 100644
index 0000000..c8c18cf
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h
@@ -0,0 +1,240 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_DATA_BUILDER_H__
+#define INV_DATA_BUILDER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Uncomment this flag to enable playback debug and record or playback scenarios
+//#define INV_PLAYBACK_DBG
+
+/** This is a new sample of accel data */
+#define INV_ACCEL_NEW 1
+/** This is a new sample of gyro data */
+#define INV_GYRO_NEW 2
+/** This is a new sample of compass data */
+#define INV_MAG_NEW 4
+/** This is a new sample of temperature data */
+#define INV_TEMP_NEW 8
+/** This is a new sample of quaternion data */
+#define INV_QUAT_NEW 16
+/** Set if quaternion is 6-axis from DMP */
+#define INV_QUAT_6AXIS 1024
+/** Set if quaternion is 3-axis from DMP */
+#define INV_QUAT_3AXIS 4096
+
+/** Set if the data is contiguous. Typically not set if a sample was skipped */
+#define INV_CONTIGUOUS 16
+/** Set if the calibrated data has been solved for */
+#define INV_CALIBRATED 32
+/* INV_NEW_DATA set for a new set of data, cleared if not available. */
+#define INV_NEW_DATA 64
+/* Set if raw data exists */
+#define INV_RAW_DATA 128
+/* Set if the sensor is on */
+#define INV_SENSOR_ON 256
+/* Set if quaternion has bias correction applied */
+#define INV_BIAS_APPLIED 512
+
+#define INV_PRIORITY_MOTION_NO_MOTION          100
+#define INV_PRIORITY_GYRO_TC                   150
+#define INV_PRIORITY_QUATERNION_GYRO_ACCEL     200
+#define INV_PRIORITY_QUATERNION_NO_GYRO        250
+#define INV_PRIORITY_MAGNETIC_DISTURBANCE      300
+#define INV_PRIORITY_HEADING_FROM_GYRO         350
+#define INV_PRIORITY_COMPASS_BIAS_W_GYRO       375
+#define INV_PRIORITY_COMPASS_VECTOR_CAL        400
+#define INV_PRIORITY_COMPASS_ADV_BIAS          500
+#define INV_PRIORITY_9_AXIS_FUSION             600
+#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS  700
+#define INV_PRIORITY_QUATERNION_ACCURACY       750
+#define INV_PRIORITY_RESULTS_HOLDER            800
+#define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850
+#define INV_PRIORITY_HAL_OUTPUTS               900
+#define INV_PRIORITY_GLYPH                     950
+#define INV_PRIORITY_SHAKE                     975
+#define INV_PRIORITY_SM                        1000
+
+struct inv_single_sensor_t {
+    /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when
+    * the rotation matrix could be thought of only having elements of 0,1,-1.
+    * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign.
+    * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row.
+    * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row.
+    * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row.
+    */
+    int orientation;
+    /** The raw data in raw data units in the mounting frame */
+    short raw[3];
+    /** Raw data in body frame */
+    long raw_scaled[3];
+    /** Calibrated data */
+    long calibrated[3];
+    long sensitivity;
+    /** Sample rate in microseconds */
+    long sample_rate_us;
+    long sample_rate_ms;
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */
+    int accuracy;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    /** Bandwidth in Hz */
+    int bandwidth;
+};
+struct inv_quat_sensor_t {
+    long raw[4];
+    /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
+    * skipped due to power savings turning off this sensor.
+    * INV_NEW_DATA set for a new set of data, cleared if not available.
+    * INV_CALIBRATED_SET if calibrated data has been solved for */
+    int status;
+    inv_time_t timestamp;
+    inv_time_t timestamp_prev;
+    long sample_rate_us;
+    long sample_rate_ms;
+};
+
+struct inv_sensor_cal_t {
+    struct inv_single_sensor_t gyro;
+    struct inv_single_sensor_t accel;
+    struct inv_single_sensor_t compass;
+    struct inv_single_sensor_t temp;
+    struct inv_quat_sensor_t quat;
+    /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate
+    * which data is a new sample as these data points may have different sample rates.
+    */
+    int status;
+};
+
+// Useful for debug record and playback
+typedef enum {
+    RD_NO_DEBUG,
+    RD_RECORD,
+    RD_PLAYBACK
+} rd_dbg_mode;
+
+typedef enum {
+    PLAYBACK_DBG_TYPE_GYRO,
+    PLAYBACK_DBG_TYPE_ACCEL,
+    PLAYBACK_DBG_TYPE_COMPASS,
+    PLAYBACK_DBG_TYPE_TEMPERATURE,
+    PLAYBACK_DBG_TYPE_EXECUTE,
+    PLAYBACK_DBG_TYPE_A_ORIENT,
+    PLAYBACK_DBG_TYPE_G_ORIENT,
+    PLAYBACK_DBG_TYPE_C_ORIENT,
+    PLAYBACK_DBG_TYPE_A_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_C_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_G_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_GYRO_OFF,
+    PLAYBACK_DBG_TYPE_ACCEL_OFF,
+    PLAYBACK_DBG_TYPE_COMPASS_OFF,
+    PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE,
+    PLAYBACK_DBG_TYPE_QUAT
+
+} inv_rd_dbg_states;
+
+/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
+#define INV_MAX_DATA_CB 20
+
+#ifdef INV_PLAYBACK_DBG
+#include <stdio.h>
+void inv_turn_on_data_logging(FILE *file);
+void inv_turn_off_data_logging();
+#endif
+
+void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity);
+void inv_set_accel_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_compass_orientation_and_scale(int orientation,
+        long sensitivity);
+void inv_set_gyro_sample_rate(long sample_rate_us);
+void inv_set_compass_sample_rate(long sample_rate_us);
+void inv_set_quat_sample_rate(long sample_rate_us);
+void inv_set_accel_sample_rate(long sample_rate_us);
+void inv_set_gyro_bandwidth(int bandwidth_hz);
+void inv_set_accel_bandwidth(int bandwidth_hz);
+void inv_set_compass_bandwidth(int bandwidth_hz);
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
+
+inv_error_t inv_register_data_cb(inv_error_t (*func)
+                                 (struct inv_sensor_cal_t * data), int priority,
+                                 int sensor_type);
+inv_error_t inv_unregister_data_cb(inv_error_t (*func)
+                                   (struct inv_sensor_cal_t * data));
+
+inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp);
+inv_error_t inv_build_compass(const long *compass, int status,
+                                  inv_time_t timestamp);
+inv_error_t inv_build_accel(const long *accel, int status,
+                            inv_time_t timestamp);
+inv_error_t inv_build_temp(const long temp, inv_time_t timestamp);
+inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp);
+inv_error_t inv_execute_on_data(void);
+
+void inv_get_compass_bias(long *bias);
+
+void inv_set_compass_bias(const long *bias, int accuracy);
+void inv_set_compass_disturbance(int dist);
+void inv_set_gyro_bias(const long *bias, int accuracy);
+void inv_set_accel_bias(const long *bias, int accuracy);
+void inv_set_accel_accuracy(int accuracy);
+void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
+
+void inv_get_gyro_bias(long *bias, long *temp);
+void inv_get_accel_bias(long *bias, long *temp);
+
+void inv_gyro_was_turned_off(void);
+void inv_accel_was_turned_off(void);
+void inv_compass_was_turned_off(void);
+void inv_quaternion_sensor_was_turned_off(void);
+inv_error_t inv_init_data_builder(void);
+long inv_get_gyro_sensitivity(void);
+long inv_get_accel_sensitivity(void);
+long inv_get_compass_sensitivity(void);
+
+void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+
+void inv_get_gyro(long *gyro);
+
+int inv_get_gyro_accuracy(void);
+int inv_get_accel_accuracy(void);
+int inv_get_mag_accuracy(void);
+
+int inv_get_compass_on(void);
+int inv_get_gyro_on(void);
+int inv_get_accel_on(void);
+
+inv_time_t inv_get_last_timestamp(void);
+int inv_get_compass_disturbance(void);
+
+//New DMP Cal Functions
+inv_error_t inv_get_gyro_orient(int *orient);
+inv_error_t inv_get_accel_orient(int *orient);
+
+// internal
+int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* INV_DATA_BUILDER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
new file mode 100644
index 0000000..caa1db7
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -0,0 +1,497 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+/**

+ *   @defgroup  HAL_Outputs hal_outputs

+ *   @brief     Motion Library - HAL Outputs

+ *              Sets up common outputs for HAL

+ *

+ *   @{

+ *       @file  hal_outputs.c

+ *       @brief HAL Outputs.

+ */

+

+#include <string.h>

+

+#include "hal_outputs.h"

+#include "log.h"

+#include "ml_math_func.h"

+#include "mlmath.h"

+#include "start_manager.h"

+#include "data_builder.h"

+#include "results_holder.h"

+

+struct hal_output_t {

+    int accuracy_mag;    /**< Compass accuracy */

+//    int accuracy_gyro;   /**< Gyro Accuracy */

+//    int accuracy_accel;  /**< Accel Accuracy */

+    int accuracy_quat;   /**< quat Accuracy */

+

+    inv_time_t nav_timestamp;

+    inv_time_t gam_timestamp;

+//    inv_time_t accel_timestamp;

+    inv_time_t mag_timestamp;

+    long nav_quat[4];

+    int gyro_status;

+    int accel_status;

+    int compass_status;

+    int nine_axis_status;

+    inv_biquad_filter_t lp_filter[3];

+    float compass_float[3];

+    long linear_acceleration_sample_rate_us;

+    long gravity_sample_rate_us;

+};

+

+static struct hal_output_t hal_out;

+

+void inv_set_linear_acceleration_sample_rate(long sample_rate_us)

+{

+    hal_out.linear_acceleration_sample_rate_us = sample_rate_us;

+}

+

+void inv_set_gravity_sample_rate(long sample_rate_us)

+{

+    hal_out.gravity_sample_rate_us = sample_rate_us;

+}

+

+/** Acceleration (m/s^2) in body frame.

+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it

+*             should return a vector of magnitude near 9.81 m/s^2

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,

+                                       inv_time_t * timestamp)

+{

+    int status;

+    /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.

+     * So this 9.80665 / 2^16 */

+#define ACCEL_CONVERSION 0.000149637603759766f

+    long accel[3];

+    inv_get_accel_set(accel, accuracy, timestamp);

+    values[0] = accel[0] * ACCEL_CONVERSION;

+    values[1] = accel[1] * ACCEL_CONVERSION;

+    values[2] = accel[2] * ACCEL_CONVERSION;

+    if (hal_out.accel_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/** Linear Acceleration (m/s^2) in Body Frame.

+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show

+*             accel biases while at rest.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    long gravity[3], accel[3];

+    inv_time_t timestamp1;

+

+    inv_get_accel_set(accel, accuracy, &timestamp1);

+    inv_get_gravity(gravity);

+    accel[0] -= gravity[0] >> 14;

+    accel[1] -= gravity[1] >> 14;

+    accel[2] -= gravity[2] >> 14;

+    values[0] = accel[0] * ACCEL_CONVERSION;

+    values[1] = accel[1] * ACCEL_CONVERSION;

+    values[2] = accel[2] * ACCEL_CONVERSION;

+

+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);

+}

+

+/** Gravity vector (m/s^2) in Body Frame.

+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_accel().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,

+                                 inv_time_t * timestamp)

+{

+    long gravity[3];

+

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    inv_get_gravity(gravity);

+    values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;

+    values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;

+    values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;

+    return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);

+}

+

+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.

+ * So this is: pi / 2^16 / 180 */

+#define GYRO_CONVERSION 2.66316109007924e-007f

+

+/** Gyroscope calibrated data (rad/s) in body frame.

+* @param[out] values Rotation Rate in rad/sec.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_gyro().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,

+                                   inv_time_t * timestamp)

+{

+    long gyro[3];

+    int status;

+

+    inv_get_gyro_set(gyro, accuracy, timestamp);

+    values[0] = gyro[0] * GYRO_CONVERSION;

+    values[1] = gyro[1] * GYRO_CONVERSION;

+    values[2] = gyro[2] * GYRO_CONVERSION;

+    if (hal_out.gyro_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/** Gyroscope raw data (rad/s) in body frame.

+* @param[out] values Rotation Rate in rad/sec.

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to

+*             inv_build_gyro().

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,

+                                   inv_time_t * timestamp)

+{

+    long gyro[3];

+    int status;

+

+    inv_get_gyro_set_raw(gyro, accuracy, timestamp);

+    values[0] = gyro[0] * GYRO_CONVERSION;

+    values[1] = gyro[1] * GYRO_CONVERSION;

+    values[2] = gyro[2] * GYRO_CONVERSION;

+    if (hal_out.gyro_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+/**

+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.

+* The rotation vector represents the orientation of the device as a combination

+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$

+* around an axis {x, y, z}. <br>

+* The three elements of the rotation vector are

+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation

+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is

+* equal to the direction of the axis of rotation.

+*

+* The three elements of the rotation vector are equal to the last three components of a unit quaternion

+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).

+*

+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.

+* The reference coordinate system is defined as a direct orthonormal basis, where:

+

+    -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).

+    -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.

+    -Z points towards the sky and is perpendicular to the ground.

+* @param[out] values Length 4.

+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate

+* @param[out] timestamp Timestamp. In (ns) for Android.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,

+        inv_time_t * timestamp)

+{

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+

+    if (hal_out.nav_quat[0] >= 0) {

+        values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;

+        values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;

+        values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;

+        values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;

+    } else {

+        values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;

+        values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;

+        values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;

+        values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;

+    }

+    values[4] = inv_get_heading_confidence_interval();

+

+    return hal_out.nine_axis_status;

+}

+

+

+/** Compass data (uT) in body frame.

+* @param[out] values Compass data in (uT), length 3. May be calibrated by having

+*             biases removed and sensitivity adjusted

+* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate

+* @param[out] timestamp Timestamp. In (ns) for Android.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,

+                                        inv_time_t * timestamp)

+{

+    int status;

+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.

+     * So this is: 1 / 2^16*/

+//#define COMPASS_CONVERSION 1.52587890625e-005f

+    int i;

+

+    *timestamp = hal_out.mag_timestamp;

+    *accuracy = (int8_t) hal_out.accuracy_mag;

+

+    for (i=0; i<3; i++)  {

+        values[i] = hal_out.compass_float[i];

+    }

+    if (hal_out.compass_status & INV_NEW_DATA)

+        status = 1;

+    else

+        status = 0;

+    return status;

+}

+

+static void inv_get_rotation(float r[3][3])

+{

+    long rot[9];

+    float conv = 1.f / (1L<<30);

+

+    inv_quaternion_to_rotation(hal_out.nav_quat, rot);

+    r[0][0] = rot[0]*conv;

+    r[0][1] = rot[1]*conv;

+    r[0][2] = rot[2]*conv;

+    r[1][0] = rot[3]*conv;

+    r[1][1] = rot[4]*conv;

+    r[1][2] = rot[5]*conv;

+    r[2][0] = rot[6]*conv;

+    r[2][1] = rot[7]*conv;

+    r[2][2] = rot[8]*conv;

+}

+

+static void google_orientation(float *g)

+{

+    float rad2deg = (float)(180.0 / M_PI);

+    float R[3][3];

+

+    inv_get_rotation(R);

+

+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;

+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;

+    g[2] = asinf ( R[2][0])          * rad2deg;

+    if (g[0] < 0)

+        g[0] += 360;

+}

+

+

+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.

+* @param[out] values Length 3, Degrees.<br>

+*        - values[0]: Azimuth, angle between the magnetic north direction

+*         and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>

+*        - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values

+*         when the z-axis moves toward the y-axis.<br>

+*        - values[2]: Roll, rotation around y-axis (-90 to 90), with positive

+*          values when the x-axis moves toward the z-axis.<br>

+*

+* @note  This definition is different from yaw, pitch and roll used in aviation

+*        where the X axis is along the long side of the plane (tail to nose).

+*        Note: This sensor type exists for legacy reasons, please use getRotationMatrix()

+*        in conjunction with remapCoordinateSystem() and getOrientation() to compute

+*        these values instead.

+*        Important note: For historical reasons the roll angle is positive in the

+*        clockwise direction (mathematically speaking, it should be positive in

+*        the counter-clockwise direction).

+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.

+* @param[out] timestamp The timestamp for this sensor.

+* @return     Returns 1 if the data was updated or 0 if it was not updated.

+*/

+int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,

+                                     inv_time_t * timestamp)

+{

+    *accuracy = (int8_t) hal_out.accuracy_quat;

+    *timestamp = hal_out.nav_timestamp;

+

+    google_orientation(values);

+

+    return hal_out.nine_axis_status;

+}

+

+/** Main callback to generate HAL outputs. Typically not called by library users.

+* @param[in] sensor_cal Input variable to take sensor data whenever there is new

+* sensor data.

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

+*/

+inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)

+{

+    int use_sensor = 0;

+    long sr = 1000;

+    long compass[3];

+    int8_t accuracy;

+    int i;

+    (void) sensor_cal;

+

+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,

+                           &hal_out.nav_timestamp);

+    hal_out.gyro_status = sensor_cal->gyro.status;

+    hal_out.accel_status = sensor_cal->accel.status;

+    hal_out.compass_status = sensor_cal->compass.status;

+

+    // Find the highest sample rate and tie generating 9-axis to that one.

+    if (sensor_cal->gyro.status & INV_SENSOR_ON) {

+        sr = sensor_cal->gyro.sample_rate_ms;

+        use_sensor = 0;

+    }

+    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {

+        sr = sensor_cal->accel.sample_rate_ms;

+        use_sensor = 1;

+    }

+    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {

+        sr = sensor_cal->compass.sample_rate_ms;

+        use_sensor = 2;

+    }

+    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {

+        sr = sensor_cal->quat.sample_rate_ms;

+        use_sensor = 3;

+    }

+

+    // Only output 9-axis if all 9 sensors are on.

+    if (sensor_cal->quat.status & INV_SENSOR_ON) {

+        // If quaternion sensor is on, gyros are not required as quaternion already has that part

+        if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {

+            use_sensor = -1;

+        }

+    } else {

+        if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {

+            use_sensor = -1;

+        }

+    }

+

+    switch (use_sensor) {

+    case 0:

+        hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->gyro.timestamp;

+        break;

+    case 1:

+        hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->accel.timestamp;

+        break;

+    case 2:

+        hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->compass.timestamp;

+        break;

+    case 3:

+        hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;

+        hal_out.nav_timestamp = sensor_cal->quat.timestamp;

+        break;

+    default:

+        hal_out.nine_axis_status = 0; // Don't output quaternion related info

+        break;

+    }

+

+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.

+     * So this is: 1 / 2^16*/

+    #define COMPASS_CONVERSION 1.52587890625e-005f

+

+    inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );

+    hal_out.accuracy_mag = (int ) accuracy;

+

+    for (i=0; i<3; i++) {

+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==

+                                                             INV_NEW_DATA )  {

+            // set the state variables to match output with input

+            inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);

+        }

+

+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==

+                                         (INV_NEW_DATA | INV_RAW_DATA)   )  {

+

+            hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],

+                                           (float ) compass[i]) * COMPASS_CONVERSION;

+

+        } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA )  {

+            hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;

+        }

+

+    }

+    return INV_SUCCESS;

+}

+

+/** Turns off generation of HAL outputs.

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

+ */

+inv_error_t inv_stop_hal_outputs(void)

+{

+    inv_error_t result;

+    result = inv_unregister_data_cb(inv_generate_hal_outputs);

+    return result;

+}

+

+/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()

+* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().

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

+*/

+inv_error_t inv_start_hal_outputs(void)

+{

+    inv_error_t result;

+    result =

+        inv_register_data_cb(inv_generate_hal_outputs,

+                             INV_PRIORITY_HAL_OUTPUTS,

+                             INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);

+    return result;

+}

+/* file name: lowPassFilterCoeff_1_6.c */

+float compass_low_pass_filter_coeff[5] =

+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};

+

+/** Initializes hal outputs class. This is called automatically by the

+* enable function. It may be called any time the feature is enabled, but

+* is typically not needed to be called by outside callers.

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

+*/

+inv_error_t inv_init_hal_outputs(void)

+{

+    int i;

+    memset(&hal_out, 0, sizeof(hal_out));

+    for (i=0; i<3; i++)  {

+        inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);

+    }

+

+    return INV_SUCCESS;

+}

+

+/** Turns on creation and storage of HAL type results.

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

+*/

+inv_error_t inv_enable_hal_outputs(void)

+{

+    inv_error_t result;

+

+	// don't need to check the result for inv_init_hal_outputs

+	// since it's always INV_SUCCESS

+	inv_init_hal_outputs();

+

+    result = inv_register_mpl_start_notification(inv_start_hal_outputs);

+    return result;

+}

+

+/** Turns off creation and storage of HAL type results.

+*/

+inv_error_t inv_disable_hal_outputs(void)

+{

+    inv_error_t result;

+

+    inv_stop_hal_outputs(); // Ignore error if we have already stopped this

+    result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);

+    return result;

+}

+

+/**

+ * @}

+ */

diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
new file mode 100644
index 0000000..41b72c6
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -0,0 +1,49 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#include "mltypes.h"
+
+#ifndef INV_HAL_OUTPUTS_H__
+#define INV_HAL_OUTPUTS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+                                         inv_time_t * timestamp);
+    int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+                                           inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
+    int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
+                                            inv_time_t * timestamp);
+    int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
+            inv_time_t * timestamp);
+
+    int inv_get_sensor_type_linear_acceleration(float *values,
+            int8_t *accuracy,
+            inv_time_t * timestamp);
+    int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+                                     inv_time_t * timestamp);
+
+    inv_error_t inv_enable_hal_outputs(void);
+    inv_error_t inv_disable_hal_outputs(void);
+    inv_error_t inv_init_hal_outputs(void);
+    inv_error_t inv_start_hal_outputs(void);
+    inv_error_t inv_stop_hal_outputs(void);
+
+    // Set data rates for virtual sensors
+    void inv_set_linear_acceleration_sample_rate(long sample_rate_us);
+    void inv_set_gravity_sample_rate(long sample_rate_us);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif                          // INV_HAL_OUTPUTS_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h
new file mode 100644
index 0000000..fb8e12b
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/invensense.h
@@ -0,0 +1,28 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+/**
+ *  Main header file for Invensense's basic library.
+ */
+
+#include "data_builder.h"
+#include "hal_outputs.h"
+#include "message_layer.h"
+#include "mlmath.h"
+#include "ml_math_func.h"
+#include "mpl.h"
+#include "results_holder.h"
+#include "start_manager.h"
+#include "storage_manager.h"
+#include "log.h"
+#include "mlinclude.h"
+//#include "..\HAL\include\mlos.h"
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
new file mode 100644
index 0000000..72940f7
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -0,0 +1,279 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_LOAD_DMP
+ *
+ * @{
+ *      @file     ml_load_dmp.c
+ *      @brief    functions for writing dmp firmware.
+ */
+#include <stdio.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-loaddmp"
+
+#include "ml_load_dmp.h"
+#include "mlos.h"
+
+#define LOADDMP_LOG MPL_LOGI
+#define LOADDMP_LOG MPL_LOGI
+
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+#define DMP_CODE_SIZE 3065
+
+static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
+   /* bank # 0 */
+    0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 
+    0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 
+    0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 
+    0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 
+    0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 
+    0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, 
+    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 
+    0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 
+    0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 
+    0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 
+    0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, 
+    /* bank # 1 */
+    0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 
+    0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 
+    0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 
+    0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 
+    0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 
+    0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 
+    0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 
+    0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 
+    0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 
+    /* bank # 2 */
+    0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 
+    0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, 
+    0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 
+    0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 
+    0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 
+    0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    /* bank # 3 */
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 
+    0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+
+    /* bank # 4 */
+    0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 
+    0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 
+    0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, 
+    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, 
+    0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 
+    0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
+    0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 
+    0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 
+    0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, 
+    0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 
+    0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 
+    0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 
+    0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 
+    0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 
+    0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, 
+    0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 
+    /* bank # 5 */
+    0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 
+    0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 
+    0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 
+    0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 
+    0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 
+    0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 
+    0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 
+    0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 
+    0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 
+    0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 
+    0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 
+    0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, 
+    0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, 
+    0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, 
+    0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, 
+    0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, 
+    /* bank # 6 */
+    0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, 
+    0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, 
+    0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, 
+    0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, 
+    0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 
+    0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, 
+    0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, 
+    0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, 
+    0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, 
+    0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, 
+    0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, 
+    0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, 
+    0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, 
+    0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, 
+    0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, 
+    0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, 
+    /* bank # 7 */
+    0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, 
+    0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, 
+    0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, 
+    0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, 
+    0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, 
+    0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, 
+    0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, 
+    0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, 
+    0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, 
+    0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, 
+    0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, 
+    0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, 
+    0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 
+    0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, 
+    0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, 
+    0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, 
+    /* bank # 8 */
+    0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, 
+    0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, 
+    0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, 
+    0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, 
+    0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, 
+    0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, 
+    0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0, 
+    0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, 
+    0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, 
+    0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, 
+    0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, 
+    0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, 
+    0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, 
+    0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, 
+    0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, 
+    0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, 
+    /* bank # 9 */
+    0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, 
+    0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, 
+    0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, 
+    0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, 
+    0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, 
+    0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, 
+    0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, 
+    0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, 
+    0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, 
+    0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, 
+    0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, 
+    0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, 
+    0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, 
+    0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, 
+    0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 
+    0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 
+    /* bank # 10 */
+    0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, 
+    0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 
+    0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 
+    0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 
+    0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 
+    0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 
+    0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 
+    0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 
+    0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 
+    0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 
+    0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 
+    0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 
+    0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 
+    0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 
+    /* bank # 11 */
+    0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 
+    0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 
+    0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 
+    0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 
+    0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 
+    0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 
+    0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 
+    0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 
+    0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 
+    0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 
+    0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 
+    0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 
+    0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 
+    0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 
+    0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 
+    0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
+};
+
+#define DMP_VERSION (dmpMemory)
+
+inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
+{
+    inv_error_t result = INV_SUCCESS;
+    int bytesWritten = 0;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    } else {
+        MPL_LOGI("dmp firmware size to write = %d", len);
+    }
+    if (fd == NULL) {
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(dmp, 1, len, fd);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    } else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    return result;
+}
+
+inv_error_t inv_load_dmp(FILE *fd)
+{
+    inv_error_t result = INV_SUCCESS;
+    result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE);
+    return result;
+}
+
+/**
+ *  @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
new file mode 100644
index 0000000..4d98692
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef INV_LOAD_DMP_H
+#define INV_LOAD_DMP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_dmp(FILE  *fd);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_LOAD_DMP_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
new file mode 100644
index 0000000..b4c4249
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -0,0 +1,353 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ * @defgroup ML_STORED_DATA
+ *
+ * @{
+ *      @file     ml_stored_data.c
+ *      @brief    functions for reading and writing stored data sets.
+ *                Typically, these functions process stored calibration data.
+ */
+
+#include <stdio.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-storeload"
+
+#include "ml_stored_data.h"
+#include "storage_manager.h"
+#include "mlos.h"
+
+#define LOADCAL_DEBUG    0
+#define STORECAL_DEBUG   0
+
+#define DEFAULT_KEY 29681
+
+#define STORECAL_LOG MPL_LOGI
+#define LOADCAL_LOG  MPL_LOGI
+
+inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
+{
+    FILE *fp;
+    inv_error_t result = INV_SUCCESS;
+    size_t fsize;
+
+    fp = fopen(MLCAL_FILE,"rb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+
+    // obtain file size
+    fseek (fp, 0 , SEEK_END);
+    fsize = ftell (fp);
+    rewind (fp);
+  
+    *calData = (unsigned char *)inv_malloc(fsize);
+    if (*calData==NULL) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", fsize);
+        fclose(fp);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    *bytesRead = fread(*calData, 1, fsize, fp);
+    if (*bytesRead != fsize) {
+        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
+                 *bytesRead, fsize);
+        result = INV_ERROR_FILE_READ;
+        goto read_cal_end;
+    }
+    else {
+        MPL_LOGI("Bytes read = %d", *bytesRead);
+    }
+
+read_cal_end:
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_write_cal(unsigned char *cal, size_t len)
+{
+    FILE *fp;
+    int bytesWritten;
+    inv_error_t result = INV_SUCCESS;
+   
+    if (len <= 0) {
+        MPL_LOGE("Nothing to write");
+        return INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("cal data size to write = %d", len);
+    }
+    fp = fopen(MLCAL_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(cal, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    else {
+        MPL_LOGI("Bytes written = %d", bytesWritten);
+    }
+    fclose(fp);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 0 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature compensation : temperature data points,
+ *          - temperature compensation : gyro biases data points for X, Y,
+ *              and Z axes.
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data is produced internally by the MPL and its
+ *          size is 2777 bytes (header and checksum included).
+ *          Calibration format type 1 is currently used for ITG3500
+ *
+ *  @pre    inv_init_storage_manager()
+ *          must have been called.
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
+{
+    inv_error_t result;
+
+    LOADCAL_LOG("Entering inv_load_cal_V0\n");
+
+    /*if (len != expLen) {
+        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
+                 expLen, len);
+        return INV_ERROR_FILE_READ;
+    }*/
+
+    result = inv_load_mpl_states(calData, len);
+    return result;
+}
+
+/**
+ *  @brief  Loads a type 1 set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *          This calibrations data format stores values for (in order of
+ *          appearance) :
+ *          - temperature,
+ *          - gyro biases for X, Y, Z axes,
+ *          - accel biases for X, Y, Z axes.
+ *          This calibration data would normally be produced by the MPU Self
+ *          Test and its size is 36 bytes (header and checksum included).
+ *          Calibration format type 1 is produced by the MPU Self Test and
+ *          substitutes the type 0 : inv_load_cal_V0().
+ *
+ *  @pre    
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be parsed.
+ *  @param  len
+ *              the length of the calibration
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
+{
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   Loads a set of calibration data.
+ *          It parses a binary data set containing calibration data.
+ *          The binary data set is intended to be loaded from a file.
+ *
+ * @pre     
+ *          
+ *
+ * @param   calData
+ *              A pointer to an array of bytes to be parsed.
+ *
+ * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_cal(unsigned char *calData)
+{
+    int calType = 0;
+    int len = 0;
+    //int ptr;
+    //uint32_t chk = 0;
+    //uint32_t cmp_chk = 0;
+
+    /*load_func_t loaders[] = {
+        inv_load_cal_V0,
+        inv_load_cal_V1,
+    };
+    */
+
+    inv_load_cal_V0(calData, len);
+
+    /* read the header (type and len)
+       len is the total record length including header and checksum */
+    len = 0;
+    len += 16777216L * ((int)calData[0]);
+    len += 65536L * ((int)calData[1]);
+    len += 256 * ((int)calData[2]);
+    len += (int)calData[3];
+
+    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
+    if (calType > 5) {
+        MPL_LOGE("Unsupported calibration file format %d. "
+                 "Valid types 0..5\n", calType);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    /* call the proper method to read in the data */
+    //return loaders[calType] (calData, len);
+    return 0;
+}
+
+/**
+ *  @brief  Stores a set of calibration data.
+ *          It generates a binary data set containing calibration data.
+ *          The binary data set is intended to be stored into a file.
+ *
+ *  @pre    inv_dmp_open()
+ *
+ *  @param  calData
+ *              A pointer to an array of bytes to be stored.
+ *  @param  length
+ *              The amount of bytes available in the array.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_store_cal(unsigned char *calData, size_t length)
+{
+    inv_error_t res = 0;
+    size_t size;
+
+    STORECAL_LOG("Entering inv_store_cal\n");
+
+    inv_get_mpl_state_size(&size);
+
+    MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
+
+    /* store data */
+    res = inv_save_mpl_states(calData, size);
+    if(res != 0)
+    {
+        MPL_LOGE("inv_save_mpl_states() failed");
+    }
+
+    STORECAL_LOG("Exiting inv_store_cal\n");
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Load a calibration file.
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_load_calibration(void)
+{
+    unsigned char *calData= NULL;
+    inv_error_t result = 0;
+    size_t bytesRead = 0;
+
+    result = inv_read_cal(&calData, &bytesRead);
+    if(result != INV_SUCCESS) {
+        MPL_LOGE("Could not load cal file - "
+                 "aborting\n");
+        goto free_mem_n_exit;
+    }
+
+    result = inv_load_mpl_states(calData, bytesRead);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not load the calibration data - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @brief  Store runtime calibration data to a file
+ *
+ *  @pre    Must be in INV_STATE_DMP_OPENED state.
+ *          inv_dmp_open() or inv_dmp_stop() must have been called.
+ *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
+ *          been called.
+ *
+ *  @return 0 or error code.
+ */
+inv_error_t inv_store_calibration(void)
+{
+    unsigned char *calData;
+    inv_error_t result;
+    size_t length;
+
+    result = inv_get_mpl_state_size(&length);
+    calData = (unsigned char *)inv_malloc(length);
+    if (!calData) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", length);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+    else {
+        MPL_LOGI("mpl state size = %d", length);
+    }
+
+    result = inv_save_mpl_states(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not save mpl states - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+    }
+    else {
+        MPL_LOGE("calData from inv_save_mpl_states, size=%d", 
+                 strlen((char *)calData));
+    }
+
+    result = inv_write_cal(calData, length);
+    if (result != INV_SUCCESS) {
+        MPL_LOGE("Could not store calibrated data on file - "
+                 "error %d - aborting\n", result);
+        goto free_mem_n_exit;
+
+    }
+
+free_mem_n_exit:
+    inv_free(calData);
+    return result;
+}
+
+/**
+ *  @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
new file mode 100644
index 0000000..115b34c
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
@@ -0,0 +1,53 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $
+ *
+ ******************************************************************************/
+
+#ifndef INV_MPL_STORED_DATA_H
+#define INV_MPL_STORED_DATA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Includes.
+*/
+#include "mltypes.h"
+
+/*
+    Defines
+*/
+#define MLCAL_FILE "/data/inv_cal_data.bin"
+
+/*
+    APIs
+*/
+inv_error_t inv_load_calibration(void);
+inv_error_t inv_store_calibration(void);
+
+/*
+    Internal APIs
+*/
+inv_error_t inv_read_cal(unsigned char **, size_t *);
+inv_error_t inv_write_cal(unsigned char *cal, size_t len);
+inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
+inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
+
+/*
+    Other prototypes
+*/
+inv_error_t inv_load_cal(unsigned char *calData);
+inv_error_t inv_store_cal(unsigned char *calData, size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  /* INV_MPL_STORED_DATA_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
new file mode 100644
index 0000000..d0c4513
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -0,0 +1,423 @@
+#include <string.h>
+#include <stdio.h>
+#include "ml_sysfs_helper.h"
+#include <dirent.h>
+#include <ctype.h>
+#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
+
+enum PROC_SYSFS_CMD {
+	CMD_GET_SYSFS_PATH,
+	CMD_GET_DMP_PATH,
+	CMD_GET_CHIP_NAME,
+	CMD_GET_SYSFS_KEY,
+	CMD_GET_TRIGGER_PATH,
+	CMD_GET_DEVICE_NODE
+};
+static char sysfs_path[100];
+static char *chip_name[] = {
+    "ITG3500", 
+    "MPU6050", 
+    "MPU9150", 
+    "MPU3050", 
+    "MPU6500"
+};
+static int chip_ind;
+static int initialized =0;
+static int status = 0;
+static int iio_initialized = 0;
+static int iio_dev_num = 0;
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+#define CHIP_NUM ARRAY_SIZE(chip_name)
+
+static const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * find_type_by_name() - function to match top level types by name
+ * @name: top level type instance name
+ * @type: the type of top level instance being sort
+ *
+ * Typical types this is used for are device and trigger.
+ **/
+int find_type_by_name(const char *name, const char *type)
+{
+	const struct dirent *ent;
+	int number, numstrlen;
+
+	FILE *nameFile;
+	DIR *dp;
+	char thisname[IIO_MAX_NAME_LENGTH];
+	char *filename;
+
+	dp = opendir(iio_dir);
+	if (dp == NULL) {
+		printf("No industrialio devices available");
+		return -ENODEV;
+	}
+
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name, ".") != 0 &&
+			strcmp(ent->d_name, "..") != 0 &&
+			strlen(ent->d_name) > strlen(type) &&
+			strncmp(ent->d_name, type, strlen(type)) == 0) {
+			numstrlen = sscanf(ent->d_name + strlen(type),
+					   "%d",
+					   &number);
+			/* verify the next character is not a colon */
+			if (strncmp(ent->d_name + strlen(type) + numstrlen,
+					":",
+					1) != 0) {
+				filename = malloc(strlen(iio_dir)
+						+ strlen(type)
+						+ numstrlen
+						+ 6);
+				if (filename == NULL)
+					return -ENOMEM;
+				sprintf(filename, "%s%s%d/name",
+					iio_dir,
+					type,
+					number);
+				nameFile = fopen(filename, "r");
+				if (!nameFile)
+					continue;
+				free(filename);
+				fscanf(nameFile, "%s", thisname);
+				if (strcmp(name, thisname) == 0)
+					return number;
+				fclose(nameFile);
+			}
+		}
+	}
+	return -ENODEV;
+}
+
+/* mode 0: search for which chip in the system and fill sysfs path
+   mode 1: return event number
+ */
+static int parsing_proc_input(int mode, char *name){
+	const char input[] = "/proc/bus/input/devices";
+	char line[4096], d;
+	char tmp[100];
+	FILE *fp;
+	int i, j, result, find_flag;
+	int event_number = -1;
+	int input_number = -1;
+
+	if(NULL == (fp = fopen(input, "rt")) ){
+		return -1;
+	}
+	result = 1;
+	find_flag = 0;
+	while(result != 0 && find_flag < 2){
+		i = 0;
+		d = 0;
+		memset(line, 0, 100);
+		while(d != '\n'){		
+			result = fread(&d, 1, 1, fp);
+			if(result == 0){
+				line[0] = 0;
+				break;
+			}
+			sprintf(&line[i], "%c", d);		
+			i ++;
+		}
+		if(line[0] == 'N'){
+			i = 1;
+			while(line[i] != '"'){
+				i++;
+			}
+			i++;
+			j = 0;
+			find_flag = 0;
+			if (mode == 0){
+				while(j < CHIP_NUM){
+					if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){
+						find_flag = 1;
+						chip_ind = j;
+					}
+					j++;
+				}
+			} else if (mode  != 0){
+				if(!memcmp(&line[i], name, strlen(name))){
+					find_flag = 1;
+				}
+			}
+		}		
+		if(find_flag){
+			if(mode == 0){
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}	
+					sprintf(sysfs_path, "%s%s", "/sys", tmp);
+					find_flag++;
+				}
+			} else if(mode == 1){
+				if(line[0] == 'H') {
+					i = 2;
+					while(line[i] != '=') i++;
+					while(line[i] != 't') i++;	
+					i++;
+					event_number = 0;
+					while(line[i] != '\n'){
+						if(line[i] >= '0' && line[i] <= '9')
+							event_number = event_number*10 + line[i]-0x30;
+						i ++;
+					}
+					find_flag ++;
+				}
+			} else if (mode == 2) {
+				if(line[0] == 'S'){
+					memset(tmp, 0, 100);
+					i =1;
+					while(line[i] != '=') i++;
+					i++;
+					j = 0;
+					while(line[i] != '\n'){
+						tmp[j] = line[i];
+						i ++; j++;
+					}
+					input_number = 0;
+					if(tmp[j-2] >= '0' && tmp[j-2] <= '9') 
+						input_number += (tmp[j-2]-0x30)*10;
+					if(tmp[j-1] >= '0' && tmp[j-1] <= '9') 
+						input_number += (tmp[j-1]-0x30);
+					find_flag++;
+				}
+			}
+		}
+	}
+	fclose(fp);
+	if(find_flag == 0){
+		return -1;
+	}
+	if(0 == mode)
+		status = 1;
+	if (mode == 1)
+		return event_number;
+	if (mode == 2)
+		return input_number;
+	return 0;
+
+}
+static void init_iio() {
+	int i, j;
+	char iio_chip[10];
+	int dev_num;
+	for(j=0; j< CHIP_NUM; j++) {
+		for (i=0; i<strlen(chip_name[j]); i++) {
+			iio_chip[i] = tolower(chip_name[j][i]);
+		}
+		iio_chip[strlen(chip_name[0])] = '\0';
+		dev_num = find_type_by_name(iio_chip, "iio:device");
+		if(dev_num >= 0) {
+			iio_initialized = 1;
+			iio_dev_num = dev_num;
+			chip_ind = j;
+		}
+	}
+}
+
+static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data)
+{
+	char key_path[100];
+	FILE *fp;
+	int i, result;
+	if(initialized == 0){
+		parsing_proc_input(0, NULL);
+		initialized = 1;
+	}
+	if(initialized && status == 0) {
+		init_iio();
+		if (iio_initialized == 0)
+			return -1;
+	}
+
+	memset(key_path, 0, 100);
+	switch(cmd){
+	case CMD_GET_SYSFS_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu");
+		break;
+	case CMD_GET_DMP_PATH:
+		if (iio_initialized == 1)
+			sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num);
+		else
+			sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware");
+		break;
+	case CMD_GET_CHIP_NAME:
+		sprintf(data, "%s", chip_name[chip_ind]);
+		break;
+	case CMD_GET_TRIGGER_PATH:
+		sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num);
+		break;
+	case CMD_GET_DEVICE_NODE:
+		sprintf(data, "/dev/iio:device%d", iio_dev_num);
+		break;
+	case CMD_GET_SYSFS_KEY:
+		memset(key_path, 0, 100);
+		if (iio_initialized == 1)
+			sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num);
+		else	
+			sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key");
+
+		if((fp = fopen(key_path, "rt")) == NULL)
+			return -1;
+		for(i=0;i<16;i++){
+			fscanf(fp, "%02x", &result);
+			data[i] = (char)result;
+		}
+		
+		fclose(fp);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+/** 
+ *  @brief  return sysfs key. if the key is not available
+ *          return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the key
+ *           It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_key(unsigned char *key)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the sysfs path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the sysfs
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_sysfs_path(char *name)
+{
+	if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+inv_error_t inv_get_sysfs_abs_path(char *name)
+{
+    strcpy(name, MPU_SYSFS_ABS_PATH);
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return the dmp file path. If the path is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the dmp file
+ *           path. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_dmpfile(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return the chip name. If the chip is not 
+ *          found yet. return false. So the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_chip_name(char *name)
+{
+   	if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+/** 
+ *  @brief  return event handler number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: event number store
+ */
+inv_error_t  inv_get_handler_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(1, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;	
+}
+
+/** 
+ *  @brief  return input number. If the handler number is not found
+ *          return false. the return value must be checked
+ *          to make sure the path is valid.
+ *  @unsigned char *name: This should be array big enough to hold the chip name
+ *           path(8 bytes). It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ *  @int *num: input number store
+ */
+inv_error_t  inv_get_input_number(const char *name, int *num)
+{
+	initialized = 0;
+	if ((*num = parsing_proc_input(2, (char *)name)) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else {
+		return INV_SUCCESS;
+	}	
+}
+
+/** 
+ *  @brief  return iio trigger name. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the trigger
+ *           name. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_trigger_path(char *name)
+{
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  return iio device node. If iio is not initialized, return false.
+ *          So the return must be checked to make sure the numeber is valid.
+ *  @unsigned char *name: This should be array big enough to hold the device
+ *           node. It should be zeroed before calling this function.
+ *           Or it could have unpredicable result.
+ */
+inv_error_t inv_get_iio_device_node(char *name)
+{
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
+		return INV_ERROR_NOT_OPENED;
+	else
+		return INV_SUCCESS;
+}
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
new file mode 100644
index 0000000..9470874
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
@@ -0,0 +1,36 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_SYSFS_HELPER_H__
+#define MLDMP_SYSFS_HELPER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "invensense.h"
+
+int find_type_by_name(const char *name, const char *type);
+inv_error_t inv_get_sysfs_path(char *name);
+inv_error_t inv_get_sysfs_abs_path(char *name);
+inv_error_t inv_get_dmpfile(char *name);
+inv_error_t inv_get_chip_name(char *name);
+inv_error_t inv_get_sysfs_key(unsigned char *key);
+inv_error_t inv_get_handler_number(const char *name, int *num);
+inv_error_t inv_get_input_number(const char *name, int *num);
+inv_error_t inv_get_iio_trigger_path(char *name);
+inv_error_t inv_get_iio_device_node(char *name);
+
+#ifdef __cplusplus
+}
+#endif
+#endif	/* MLDMP_SYSFS_HELPER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
new file mode 100644
index 0000000..d4f8912
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -0,0 +1,99 @@
+/*
+ $License:
+    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+#include <pthread.h>
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef pthread_mutex_t* HANDLE;
+#endif
+
+	/* ------------ */
+	/* - Defines. - */
+	/* ------------ */
+
+	/* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
+
+	/* ---------- */
+	/* - Enums. - */
+	/* ---------- */
+
+	/* --------------- */
+	/* - Structures. - */
+	/* --------------- */
+
+	/* --------------------- */
+	/* - Function p-types. - */
+	/* --------------------- */
+
+#ifndef __KERNEL__
+#include <string.h>
+	void *inv_malloc(unsigned int numBytes);
+	inv_error_t inv_free(void *ptr);
+	inv_error_t inv_create_mutex(HANDLE *mutex);
+	inv_error_t inv_lock_mutex(HANDLE mutex);
+	inv_error_t inv_unlock_mutex(HANDLE mutex);
+	FILE *inv_fopen(char *filename);
+	void inv_fclose(FILE *fp);
+
+	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)
+	{
+        (void)gfp_flags;
+		return inv_malloc((unsigned int)size);
+	}
+	static inline void *kzalloc(size_t size, unsigned int gfp_flags)
+	{
+		void *tmp = inv_malloc((unsigned int)size);
+        (void)gfp_flags;
+		if (tmp)
+			memset(tmp, 0, size);
+		return tmp;
+	}
+	static inline void kfree(void *ptr)
+	{
+		inv_free(ptr);
+	}
+	static inline void msleep(long msecs)
+	{
+		inv_sleep(msecs);
+	}
+	static inline void udelay(unsigned long usecs)
+	{
+		inv_sleep((usecs + 999) / 1000);
+	}
+#else
+#include <linux/delay.h>
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _MLOS_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
new file mode 100644
index 0000000..5424508
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -0,0 +1,190 @@
+/*
+ $License:
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ ******************************************************************************/
+
+/**
+ *  @defgroup MLOS
+ *  @brief OS Interface.
+ *
+ *  @{
+ *      @file mlos.c
+ *      @brief OS Interface.
+ */
+
+/* ------------- */
+/* - Includes. - */
+/* ------------- */
+
+#include <sys/time.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include "stdint_invensense.h"
+#include "mlos.h"
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ *  @brief  Allocate space
+ *  @param  num_bytes  number of bytes
+ *  @return pointer to allocated space
+ */
+void *inv_malloc(unsigned int num_bytes)
+{
+    // Allocate space.
+    void *alloc_ptr = malloc(num_bytes);
+    return alloc_ptr;
+}
+
+
+/**
+ *  @brief  Free allocated space
+ *  @param  ptr pointer to space to deallocate
+ *  @return error code.
+ */
+inv_error_t inv_free(void *ptr)
+{
+    if (ptr)
+        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 m_secs)
+{
+    usleep(m_secs * 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);
+}
+
+/** @} */
+
diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c
new file mode 100644
index 0000000..8317957
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/message_layer.c
@@ -0,0 +1,59 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+/**

+ *   @defgroup  Message_Layer message_layer

+ *   @brief     Motion Library - Message Layer

+ *              Holds Low Occurance messages

+ *

+ *   @{

+ *       @file message_layer.c

+ *       @brief Holds Low Occurance Messages.

+ */

+#include "message_layer.h"

+#include "log.h"

+

+struct message_holder_t {

+    long message;

+};

+

+static struct message_holder_t mh;

+

+/** Sets a message.

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

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

+*                  which is useful for mutually exclusive messages such

+*                  a motion or no motion message.

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

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

+*/

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

+{

+    if (level == 0) {

+        mh.message &= ~clear;

+        mh.message |= set;

+    }

+}

+

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

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

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

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

+* @return bit field to corresponding message.

+*/

+long inv_get_message_level_0(int clear)

+{

+    long msg;

+    msg = mh.message;

+    if (clear) {

+        mh.message = 0;

+    }

+    return msg;

+}

+

+/**

+ * @}

+ */

diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h
new file mode 100644
index 0000000..df0c0e2
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mllite/message_layer.h
@@ -0,0 +1,35 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_MESSAGE_LAYER_H__

+#define INV_MESSAGE_LAYER_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+    /* Level 0 Type Messages */

+    /** A motion event has occured */

+#define INV_MSG_MOTION_EVENT    (0x01)

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

+#define INV_MSG_NO_MOTION_EVENT (0x02)

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

+#define INV_MSG_NEW_GB_EVENT    (0x04)

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

+#define INV_MSG_NEW_CB_EVENT    (0x08)

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

+#define INV_MSG_NEW_AB_EVENT    (0x10)

+

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

+    long inv_get_message_level_0(int clear);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif  // INV_MESSAGE_LAYER_H__

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

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+/**

+ *   @defgroup  MPL mpl

+ *   @brief     Motion Library - Start Point

+ *              Initializes MPL.

+ *

+ *   @{

+ *       @file  mpl.c

+ *       @brief MPL start point.

+ */

+

+#include "storage_manager.h"

+#include "log.h"

+#include "mpl.h"

+#include "start_manager.h"

+#include "data_builder.h"

+#include "results_holder.h"

+#include "mlinclude.h"

+

+/**

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

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

+ */

+inv_error_t inv_init_mpl(void)

+{

+    inv_init_storage_manager();

+

+    /* initialize the start callback manager */

+    INV_ERROR_CHECK(inv_init_start_manager());

+

+    /* initialize the data builder */

+    INV_ERROR_CHECK(inv_init_data_builder());

+

+    INV_ERROR_CHECK(inv_enable_results_holder());

+

+    return INV_SUCCESS;

+}

+

+const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9";

+

+/**

+ *  @brief  used to get the MPL version.

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

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

+ */

+inv_error_t inv_get_version(char **version)

+{

+    INVENSENSE_FUNC_START;

+    /* cast out the const */

+    *version = (char *)&ml_ver;

+    return INV_SUCCESS;

+}

+

+/** 

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

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

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

+ */

+inv_error_t inv_start_mpl(void)

+{

+    INV_ERROR_CHECK(inv_execute_mpl_start_notification());

+    return INV_SUCCESS;

+}

+

+/**

+ * @}

+ */

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

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+/*******************************************************************************

+ *

+ * $Id:$

+ *

+ ******************************************************************************/

+/**

+ *   @defgroup  Start_Manager start_manager

+ *   @brief     Motion Library - Start Manager

+ *              Start Manager

+ *

+ *   @{

+ *       @file start_manager.c

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

+ */

+

+

+#include <string.h>

+#include "log.h"

+#include "start_manager.h"

+

+typedef inv_error_t (*inv_start_cb_func)();

+struct inv_start_cb_t {

+    int num_cb;

+    inv_start_cb_func start_cb[INV_MAX_START_CB];

+};

+

+static struct inv_start_cb_t inv_start_cb;

+

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

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

+*/

+inv_error_t inv_init_start_manager(void)

+{

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

+    return INV_SUCCESS;

+}

+

+/** Removes a callback from start notification

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

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

+*/

+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))

+{

+    int kk;

+

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

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

+            // Found the match

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

+                memmove(&inv_start_cb.start_cb[kk],

+                    &inv_start_cb.start_cb[kk+1],

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

+            }

+            inv_start_cb.num_cb--;

+            return INV_SUCCESS;

+        }

+    }

+    return INV_ERROR_INVALID_PARAMETER;

+}

+

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

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

+*            called.

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

+*/

+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))

+{

+    if (inv_start_cb.num_cb >= INV_MAX_START_CB)

+        return INV_ERROR_INVALID_PARAMETER;

+

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

+    inv_start_cb.num_cb++;

+    return INV_SUCCESS;

+}

+

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

+* called.

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

+*/

+inv_error_t inv_execute_mpl_start_notification(void)

+{

+    inv_error_t result,first_error;

+    int kk;

+

+    first_error = INV_SUCCESS;

+

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

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

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

+            first_error = result;

+        }

+    }

+    return first_error;

+}

+

+/**

+ * @}

+ */

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

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+/*******************************************************************************

+ *

+ * $Id$

+ *

+ ******************************************************************************/

+

+#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__

+#define MLDMP_ACCEL_AUTO_CALIBRATION_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+inv_error_t inv_enable_in_use_auto_calibration(void);

+inv_error_t inv_disable_in_use_auto_calibration(void);

+inv_error_t inv_stop_in_use_auto_calibration(void);

+inv_error_t inv_start_in_use_auto_calibration(void);

+inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);

+inv_error_t inv_init_in_use_auto_calibration(void);

+void inv_init_accel_maxmin(void);

+void inv_record_good_accel_maxmin(void);

+int inv_get_accel_bias_stage();

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__

+

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

+ $License:

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

+    See included License.txt for License information.

+ $

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

+ $License:

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

+    See included License.txt for License information.

+ $

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

+    inv_error_t inv_stop_fast_nomot(void);

+    inv_error_t inv_init_fast_nomot(void);

+    void inv_set_default_number_of_samples(int N);

+    inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);

+    inv_error_t inv_update_fast_nomot(long *gyro);

+

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

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

+    void inv_set_fast_nomot_accel_threshold(long long thresh);

+    void inv_set_fast_nomot_compass_threshold(long long thresh);

+    void int_set_fast_nomot_gyro_threshold(long long thresh);

+

+    void inv_fnm_debug_print(void);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif // MLDMP_FAST_NO_MOTION_H__

+

diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
new file mode 100644
index 0000000..1ba1ebb
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -0,0 +1,37 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_FUSION9AXIS_H__
+#define MLDMP_FUSION9AXIS_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void inv_init_9x_fusion(void);
+    inv_error_t inv_9x_fusion_state_change(unsigned char newState);
+    inv_error_t inv_enable_9x_sensor_fusion(void);
+    inv_error_t inv_disable_9x_sensor_fusion(void);
+    inv_error_t inv_start_9x_sensor_fusion(void);
+    inv_error_t inv_stop_9x_sensor_fusion(void);
+    inv_error_t inv_9x_fusion_set_mag_fb(double fb);

+    inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);

+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FUSION9AXIS_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
new file mode 100644
index 0000000..69918d5
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
@@ -0,0 +1,43 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

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

+inv_error_t inv_stop_gyro_tc(void);
+
+inv_error_t inv_get_gyro_ts(long *data);
+inv_error_t inv_set_gyro_ts(long *data);
+
+inv_error_t inv_init_gyro_ts(void);
+
+inv_error_t inv_set_gtc_max_temp(long data);
+inv_error_t inv_set_gtc_min_temp(long data);
+
+inv_error_t inv_print_gtc_data(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* _GYRO_TC_H */
+
diff --git a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
new file mode 100644
index 0000000..09ecc42
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _HEADING_FROM_GYRO_H_
+#define _HEADING_FROM_GYRO_H_
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    inv_error_t inv_enable_heading_from_gyro(void);
+    inv_error_t inv_disable_heading_from_gyro(void);
+    void inv_init_heading_from_gyro(void);
+    inv_error_t inv_start_heading_from_gyro(void);
+    inv_error_t inv_stop_heading_from_gyro(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  /* _HEADING_FROM_GYRO_H_ */
diff --git a/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h
new file mode 100644
index 0000000..6620bbf
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/inv_math.h
@@ -0,0 +1,8 @@
+/* math.h has many functions and defines that are not consistent across 

+* platforms. This address that */

+

+#ifdef _WINDOWS

+#define _USE_MATH_DEFINES

+#endif

+

+#include <math.h>

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

+ $License:

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

+    See included License.txt for License information.

+ $

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

+
+    float Mag3ofNormalizedLong(const long *x);

+

+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_MAGDISTURB_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
new file mode 100644
index 0000000..01cf1c0
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -0,0 +1,28 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */
+#ifndef INV_MOTION_NO_MOTION_H__

+#define INV_MOTION_NO_MOTION_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+inv_error_t inv_enable_motion_no_motion(void);

+inv_error_t inv_disable_motion_no_motion(void);

+inv_error_t inv_init_motion_no_motion(void);

+inv_error_t inv_start_motion_no_motion(void);

+inv_error_t inv_stop_motion_no_motion(void);

+

+inv_error_t inv_set_no_motion_time(long time_ms);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_MOTION_NO_MOTION_H__

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

+ $License:

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

+    See included License.txt for License information.

+ $

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

+    inv_error_t inv_start_no_gyro_fusion(void);

+    inv_error_t inv_init_no_gyro_fusion(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_NOGYROFUSION_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
new file mode 100644
index 0000000..3c6a2c1
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -0,0 +1,71 @@
+/*

+	quat_accuracy_monitor.h

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+

+/*******************************************************************************

+ *

+ * $Id:$

+ *

+ ******************************************************************************/

+

+#ifndef QUAT_ACCURARCY_MONITOR_H__

+#define QUAT_ACCURARCY_MONITOR_H__

+

+#include "mltypes.h"

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+enum accuracy_signal_type_e {

+    TYPE_NAV_QUAT,

+    TYPE_GAM_QUAT,

+    TYPE_NAV_QUAT_ADVANCED,

+    TYPE_GAM_QUAT_ADVANCED,

+    TYPE_NAV_QUAT_BASIC,

+    TYPE_GAM_QUAT_BASIC,

+    TYPE_MAG,

+    TYPE_GYRO,

+    TYPE_ACCEL,

+};

+

+inv_error_t inv_init_quat_accuracy_monitor(void);

+

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

+double get_accuracy_threshold(enum accuracy_signal_type_e type);

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

+int get_accuracy_weight(enum accuracy_signal_type_e type);

+

+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);

+

+void inv_reset_quat_accuracy(void);

+double get_6axis_correction_term(void);

+double get_9axis_correction_term(void);

+int get_9axis_accuracy_state();

+

+void set_6axis_error_average(double value);

+double get_6axis_error_bound(void);

+double get_compass_correction(void);

+double get_9axis_error_bound(void);

+

+float get_confidence_interval(void);

+void set_compass_uncertainty(float value);

+

+inv_error_t inv_enable_quat_accuracy_monitor(void);

+inv_error_t inv_disable_quat_accuracy_monitor(void);

+inv_error_t inv_start_quat_accuracy_monitor(void);

+inv_error_t inv_stop_quat_accuracy_monitor(void);

+

+double get_compassNgravity(void);

+double get_init_compassNgravity(void);

+

+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // QUAT_ACCURARCY_MONITOR_H__

diff --git a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
new file mode 100644
index 0000000..4fa36b0
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -0,0 +1,27 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_QUATERNION_SUPERVISOR_H__

+#define INV_QUATERNION_SUPERVISOR_H__

+

+#include "mltypes.h"

+

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+inv_error_t inv_enable_quaternion(void);

+inv_error_t inv_disable_quaternion(void);

+inv_error_t inv_init_quaternion(void);

+inv_error_t inv_start_quaternion(void);

+void inv_set_quaternion(long *quat);
+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_QUATERNION_SUPERVISOR_H__

diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h
new file mode 100644
index 0000000..67acb7b
--- /dev/null
+++ b/60xx/libsensors_iio/software/core/mpl/shake.h
@@ -0,0 +1,94 @@
+/*

+ $License:

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

+    See included License.txt for License information.

+ $

+ */

+#ifndef INV_SHAKE_H__

+#define INV_SHAKE_H__

+

+#include "mltypes.h"

+

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+	/* ------------ */

+    /* - Defines. - */

+    /* ------------ */

+

+    #define STATE_ZERO                             0

+    #define STATE_INIT_1                           1

+    #define STATE_INIT_2                           2

+    #define STATE_DETECT                           3

+

+	struct t_shake_config_params {

+		long shake_time_min_ms;

+		long shake_time_max_ms;

+		long shake_time_min;

+        long shake_time_max;

+		unsigned char shake_time_set;

+		long shake_time_saved;

+        float shake_deriv_thr;

+        int zero_cross_thr;

+        float accel_delta_min;

+        float accel_delta_max;

+		unsigned char interp_enable;

+	};

+

+	struct t_shake_state_params {

+		unsigned char state;

+        float accel_peak_high;

+        float accel_peak_low;

+        float accel_range;

+        int num_zero_cross;

+        short curr_shake_time;

+		int deriv_major_change;

+		int deriv_major_sign;

+        float accel_buffer[200];

+        float delta_buffer[200];

+	};

+

+	struct t_shake_data_params {

+		float accel_prev;

+		float accel_curr;

+		float delta_prev;

+		float delta_curr;

+		float delta_prev_buffer;

+	};

+

+	struct t_shake_results {

+		//unsigned char shake_int;

+		int shake_number;

+	};

+

+	struct t_shake_cb {

+       void (*shake_callback)(struct t_shake_results *shake_results);            

+    };

+

+

+    /* --------------------- */

+    /* - Function p-types. - */

+    /* --------------------- */

+	inv_error_t inv_enable_shake(void);

+    inv_error_t inv_disable_shake(void);

+    inv_error_t inv_init_shake(void);

+    inv_error_t inv_start_shake(void);

+	int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));

+	void inv_config_shake_time_params(long sample_time_ms);

+	void inv_set_shake_accel_delta_min(float accel_g);

+	void inv_set_shake_accel_delta_max(float accel_g);

+	void inv_set_shake_zero_cross_thresh(int num_zero_cross);

+	void inv_set_shake_deriv_thresh(float shake_deriv_thresh);

+	void inv_set_shake_time_min_ms(long time_ms);

+	void inv_set_shake_time_max_ms(long time_ms);

+	void inv_enable_shake_data_interpolation(unsigned char en);

+

+

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/60xx/mlsdk/Android.mk b/60xx/mlsdk/Android.mk
new file mode 100644
index 0000000..929a776
--- /dev/null
+++ b/60xx/mlsdk/Android.mk
@@ -0,0 +1,101 @@
+LOCAL_PATH := $(call my-dir)
+
+ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_MODULE := libmlplatform
+#modify these to point to the mpl source installation
+MLSDK_ROOT = .
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform/linux
+
+LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/kernel
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite
+
+ML_SOURCES := \
+    $(MLPLATFORM_DIR)/mlos_linux.c \
+    $(MLPLATFORM_DIR)/mlsl_linux_mpu.c
+
+LOCAL_SRC_FILES := $(ML_SOURCES)
+
+LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils
+include $(BUILD_SHARED_LIBRARY)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := libmllite
+LOCAL_MODULE_TAGS := optional
+#modify these to point to the mpl source installation
+MLSDK_ROOT = .
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
+MLLITE_DIR = $(MLSDK_ROOT)/mllite
+MPL_DIR = $(MLSDK_ROOT)/mldmp
+
+LOCAL_CFLAGS += -DNDEBUG
+LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
+LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE
+LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\"
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MPL_DIR) -I$(LOCAL_PATH)/$(MLLITE_DIR) -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/include
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlutils -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlapps/common
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite/akmd
+LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/linux
+
+# optionally apply the compass filter. this is set in
+# BoardConfig.mk
+ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true)
+LOCAL_CFLAGS += -DAPPLY_COMPASS_FILTER
+endif
+
+ML_SOURCES = \
+        $(MLLITE_DIR)/accel.c \
+        $(MLLITE_DIR)/compass.c \
+        $(MLLITE_DIR)/pressure.c \
+        $(MLLITE_DIR)/mldl_cfg_mpu.c \
+        $(MLLITE_DIR)/dmpDefault.c \
+        $(MLLITE_DIR)/ml.c \
+	$(MLLITE_DIR)/mlarray.c \
+	$(MLLITE_DIR)/mlarray_legacy.c \
+        $(MLLITE_DIR)/mlFIFO.c \
+        $(MLLITE_DIR)/mlFIFOHW.c \
+        $(MLLITE_DIR)/mlMathFunc.c \
+        $(MLLITE_DIR)/ml_stored_data.c \
+        $(MLLITE_DIR)/mlcontrol.c \
+        $(MLLITE_DIR)/mldl.c \
+        $(MLLITE_DIR)/mldmp.c \
+        $(MLLITE_DIR)/mlstates.c \
+        $(MLLITE_DIR)/mlsupervisor.c \
+        $(MLLITE_DIR)/mlBiasNoMotion.c \
+        $(MLLITE_DIR)/mlSetGyroBias.c \
+        \
+        $(MLLITE_DIR)/ml_mputest.c \
+        $(MLSDK_ROOT)/mlutils/mputest.c \
+        $(MLSDK_ROOT)/mlutils/checksum.c
+
+
+ifeq ($(HARDWARE),M_HW)
+    ML_SOURCES += $(MLLITE_DIR)/accel/mantis.c
+endif
+
+LOCAL_SRC_FILES := $(ML_SOURCES)
+LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform
+include $(BUILD_SHARED_LIBRARY)
+
+#This makes an .so from our .a
+#include $(CLEAR_VARS)
+#LOCAL_MODULE := libmpl
+#LOCAL_MODULE_TAGS := optional
+#LOCAL_SRC_FILES := mlsdk/mldmp/mpl/android/libmpl.a
+#LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform libmllite
+#LOCAL_WHOLE_STATIC_LIBRARIES := libmpl
+#LOCAL_PREBUILT_LIBS := mlsdk/mldmp/mpl/android/libmpl.a
+#include $(BUILD_SHARED_LIBRARY)
+#include $(BUILD_MULTI_PREBUILT)
+
+endif
diff --git a/60xx/mlsdk/mllite/accel.c b/60xx/mlsdk/mllite/accel.c
new file mode 100644
index 0000000..60b8d6c
--- /dev/null
+++ b/60xx/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];
+    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/60xx/mlsdk/mllite/accel.h b/60xx/mlsdk/mllite/accel.h
new file mode 100644
index 0000000..d3fbc6a
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/compass.c b/60xx/mlsdk/mllite/compass.c
new file mode 100644
index 0000000..798cb9f
--- /dev/null
+++ b/60xx/mlsdk/mllite/compass.c
@@ -0,0 +1,579 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/*******************************************************************************
+ *
+ * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $
+ *
+ *******************************************************************************/
+
+/** 
+ *  @defgroup COMPASSDL 
+ *  @brief  Motion Library - Compass Driver Layer.
+ *          Provides the interface to setup and handle an compass
+ *          connected to either the primary or the seconday I2C interface 
+ *          of the gyroscope.
+ *
+ *  @{
+ *      @file   compass.c
+ *      @brief  Compass setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+#include <stdlib.h>
+#include "compass.h"
+
+#include "ml.h"
+#include "mlinclude.h"
+#include "dmpKey.h"
+#include "mlFIFO.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "mlMathFunc.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+#define COMPASS_DEBUG 0
+
+/* --------------------- */
+/* - Global Variables. - */
+/* --------------------- */
+
+/* --------------------- */
+/* - Static Variables. - */
+/* --------------------- */
+
+/* --------------- */
+/* - Prototypes. - */
+/* --------------- */
+
+/* -------------- */
+/* - Externs.   - */
+/* -------------- */
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+static float square(float data)
+{
+    return data * data;
+}
+
+static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
+{
+    int i;
+
+    adap_filter->num = 0;
+    adap_filter->index = 0;
+    adap_filter->noise = noise;
+    adap_filter->len = len;
+
+    for (i = 0; i < adap_filter->len; ++i) {
+        adap_filter->sequence[i] = 0;
+    }
+}
+
+static int cmpfloat(const void *p1, const void *p2)
+{
+    return *(float*)p1 - *(float*)p2;
+}
+
+
+static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
+{
+    float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
+    int i;
+
+    if (adap_filter->len <= 1) {
+        return in;
+    }
+    if (adap_filter->num < adap_filter->len) {
+        adap_filter->sequence[adap_filter->index++] = in;
+        adap_filter->num++;
+        return in;
+    }
+    if (adap_filter->len <= adap_filter->index) {
+        adap_filter->index = 0;
+    }
+    adap_filter->sequence[adap_filter->index++] = in;
+
+    avg = 0;
+    for (i = 0; i < adap_filter->len; i++) {
+        avg += adap_filter->sequence[i];
+    }
+    avg /= adap_filter->len;
+
+    memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
+    qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
+    median = sorted[adap_filter->len/2];
+
+    sum = 0;
+    for (i = 0; i < adap_filter->len; i++) {
+        sum += square(avg - adap_filter->sequence[i]);
+    }
+    sum /= adap_filter->len;
+
+    if (sum <= adap_filter->noise) {
+        return median;
+    }
+
+    return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
+}
+
+static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
+{
+    thresh_filter->threshold = threshold;
+    thresh_filter->last = 0;
+}
+
+static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
+{
+    if (in < thresh_filter->last - thresh_filter->threshold
+            || thresh_filter->last + thresh_filter->threshold < in) {
+        thresh_filter->last = in;
+        return in;
+    }
+    else {
+        return thresh_filter->last;
+    }
+}
+
+static int init(yas_filter_handle_t *t)
+{
+    float noise[] = {
+        YAS_DEFAULT_FILTER_NOISE,
+        YAS_DEFAULT_FILTER_NOISE,
+        YAS_DEFAULT_FILTER_NOISE,
+    };
+    int i;
+
+    if (t == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < 3; i++) {
+        adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
+        thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
+    }
+
+    return 0;
+}
+
+static int update(yas_filter_handle_t *t, float *input, float *output)
+{
+    int i;
+
+    if (t == NULL || input == NULL || output == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < 3; i++) {
+        output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
+        output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
+    }
+
+    return 0;
+}
+
+int yas_filter_init(yas_filter_if_s *f)
+{
+    if (f == NULL) {
+        return -1;
+    }
+    f->init = init;
+    f->update = update;
+
+    return 0;
+}
+
+/**
+ *  @brief  Used to determine if a compass is
+ *          configured and used by the MPL.
+ *  @return INV_SUCCESS if the compass is present.
+ */
+unsigned char inv_compass_present(void)
+{
+    INVENSENSE_FUNC_START;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    if (NULL != mldl_cfg->compass &&
+        NULL != mldl_cfg->compass->resume &&
+        mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
+        return TRUE;
+    else
+        return FALSE;
+}
+
+/**
+ *  @brief   Query the compass slave address.
+ *  @return  The 7-bit compass slave address.
+ */
+unsigned char inv_get_compass_slave_addr(void)
+{
+    INVENSENSE_FUNC_START;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    if (NULL != mldl_cfg->pdata)
+        return mldl_cfg->pdata->compass.address;
+    else
+        return 0;
+}
+
+/**
+ *  @brief   Get the ID of the compass in use.
+ *  @return  ID of the compass in use.
+ */
+unsigned short inv_get_compass_id(void)
+{
+    INVENSENSE_FUNC_START;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    if (NULL != mldl_cfg->compass) {
+        return mldl_cfg->compass->id;
+    }
+    return ID_INVALID;
+}
+
+/**
+ *  @brief  Get a sample of compass data from the device.
+ *  @param  data
+ *              the buffer to store the compass raw data for
+ *              X, Y, and Z axes.
+ *  @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t inv_get_compass_data(long *data)
+{
+    inv_error_t result;
+    int ii;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    unsigned char *tmp = inv_obj.compass_raw_data;
+
+    if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+        return INV_ERROR_INVALID_CONFIGURATION;
+    }
+
+    if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
+        !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
+        /*--- read the compass sensor data.
+          The compass read function may return an INV_ERROR_COMPASS_* errors
+          when the data is not ready (read/refresh frequency mismatch) or 
+          the internal data sampling timing of the device was not respected. 
+          Returning the error code will make the sensor fusion supervisor 
+          ignore this compass data sample. ---*/
+        result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
+                                                    inv_get_serial_handle(),
+                                                    inv_get_serial_handle(),
+                                                    tmp);
+        if (result) {
+            if (COMPASS_DEBUG) {
+                MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
+            }
+            return result;
+        }
+        for (ii = 0; ii < 3; ii++) {
+            if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
+                data[ii] =
+                    ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
+            else
+                data[ii] =
+                    ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
+        }
+
+        inv_obj.compass_overunder = (int)tmp[6];
+
+    } else {
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 ||             \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+        result = inv_get_external_sensor_data(data, 3);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+        {
+            static unsigned char first = TRUE;
+            // one-off write to AKM
+            if (first) {
+                unsigned char regs[] = {
+                    // beginning Mantis register for one-off slave R/W
+                    MPUREG_I2C_SLV4_ADDR,
+                    // the slave to write to
+                    mldl_cfg->pdata->compass.address,
+                    // the register to write to
+                    /*mldl_cfg->compass->trigger->reg */ 0x0A,
+                    // the value to write
+                    /*mldl_cfg->compass->trigger->value */ 0x01,
+                    // enable the write
+                    0xC0
+                };
+                result =
+                    inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
+                                     ARRAY_SIZE(regs), regs);
+                first = FALSE;
+            } else {
+                unsigned char regs[] = {
+                    MPUREG_I2C_SLV4_CTRL,
+                    0xC0
+                };
+                result =
+                    inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
+                                     ARRAY_SIZE(regs), regs);
+            }
+        }
+#endif
+#else
+        return INV_ERROR_INVALID_CONFIGURATION;
+#endif                          // CONFIG_MPU_SENSORS_xxxx
+    }
+    data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
+    data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
+    data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
+
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  Sets the compass bias.
+ *  @param  bias 
+ *              Compass bias, length 3. Scale is micro Tesla's * 2^16. 
+ *              Frame is mount frame which may be different from body frame.
+ *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_set_compass_bias(long *bias)
+{
+    inv_error_t result = INV_SUCCESS;
+    long biasC[3];
+    struct mldl_cfg *mldlCfg = inv_get_dl_config();
+
+    inv_obj.compass_bias[0] = bias[0];
+    inv_obj.compass_bias[1] = bias[1];
+    inv_obj.compass_bias[2] = bias[2];
+
+    // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
+    biasC[0] =
+        (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
+        inv_obj.init_compass_bias[0] * (1L << 16);
+    biasC[1] =
+        (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
+        inv_obj.init_compass_bias[1] * (1L << 16);
+    biasC[2] =
+        (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
+        inv_obj.init_compass_bias[2] * (1L << 16);
+
+    if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
+        unsigned char reg[4];
+        long biasB[3];
+        signed char *orC = mldlCfg->pdata->compass.orientation;
+
+        // Now transform to body frame
+        biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
+        biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
+        biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
+
+        result =
+            inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
+                               inv_int32_to_big8(biasB[0], reg));
+        result =
+            inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
+                               inv_int32_to_big8(biasB[1], reg));
+        result =
+            inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
+                               inv_int32_to_big8(biasB[2], reg));
+    }
+    return result;
+}
+
+/**
+ *  @brief  Write a single register on the compass slave device, regardless
+ *          of the bus it is connected to and the MPU's configuration.
+ *  @param  reg
+ *              the register to write to on the slave compass device.
+ *  @param  val
+ *              the value to write.
+ *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
+{
+    struct ext_slave_config config;
+    unsigned char data[2];
+    inv_error_t result;
+
+    data[0] = reg;
+    data[1] = val;
+
+    config.key = MPU_SLAVE_WRITE_REGISTERS;
+    config.len = 2;
+    config.apply = TRUE;
+    config.data = data;
+
+    result = inv_mpu_config_compass(inv_get_dl_config(),
+                                    inv_get_serial_handle(),
+                                    inv_get_serial_handle(), &config);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  Read values from the compass slave device registers, regardless
+ *          of the bus it is connected to and the MPU's configuration.
+ *  @param  reg
+ *              the register to read from on the slave compass device.
+ *  @param  val
+ *              a buffer of 3 elements to store the values read from the 
+ *              compass device.
+ *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
+{
+    struct ext_slave_config config;
+    unsigned char data[2];
+    inv_error_t result;
+
+    data[0] = reg;
+
+    config.key = MPU_SLAVE_READ_REGISTERS;
+    config.len = 2;
+    config.apply = TRUE;
+    config.data = data;
+
+    result = inv_mpu_get_compass_config(inv_get_dl_config(),
+                                        inv_get_serial_handle(),
+                                        inv_get_serial_handle(), &config);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    *val = data[1];
+    return result;
+}
+
+/**
+ *  @brief  Read values from the compass slave device scale registers, regardless
+ *          of the bus it is connected to and the MPU's configuration.
+ *  @param  reg
+ *              the register to read from on the slave compass device.
+ *  @param  val
+ *              a buffer of 3 elements to store the values read from the 
+ *              compass device.
+ *  @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_read_scale(long *val)
+{
+    struct ext_slave_config config;
+    unsigned char data[3];
+    inv_error_t result;
+
+    config.key = MPU_SLAVE_READ_SCALE;
+    config.len = 3;
+    config.apply = TRUE;
+    config.data = data;
+
+    result = inv_mpu_get_compass_config(inv_get_dl_config(),
+                                        inv_get_serial_handle(),
+                                        inv_get_serial_handle(), &config);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    val[0] = ((data[0] - 128) << 22) + (1L << 30);
+    val[1] = ((data[1] - 128) << 22) + (1L << 30);
+    val[2] = ((data[2] - 128) << 22) + (1L << 30);
+    return result;
+}
+
+inv_error_t inv_set_compass_offset(void)
+{
+    struct ext_slave_config config;
+    unsigned char data[3];
+    inv_error_t result;
+
+    config.key = MPU_SLAVE_OFFSET_VALS;
+    config.len = 3;
+    config.apply = TRUE;
+    config.data = data;
+
+    if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
+        /* push stored values */
+        data[0] = (char)inv_obj.compass_offsets[0];
+        data[1] = (char)inv_obj.compass_offsets[1];
+        data[2] = (char)inv_obj.compass_offsets[2];
+        MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
+        result = inv_mpu_config_compass(inv_get_dl_config(),
+                                        inv_get_serial_handle(),
+                                        inv_get_serial_handle(), &config);
+    } else {
+        /* compute new values and store them */
+        result = inv_mpu_get_compass_config(inv_get_dl_config(),
+                                            inv_get_serial_handle(),
+                                            inv_get_serial_handle(), &config);
+        MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
+        if(result == INV_SUCCESS) {
+            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
+            inv_obj.compass_offsets[0] = data[0];
+            inv_obj.compass_offsets[1] = data[1];
+            inv_obj.compass_offsets[2] = data[2];
+        }
+    }
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+inv_error_t inv_compass_check_range(void)
+{
+    struct ext_slave_config config;
+    unsigned char data[3];
+    inv_error_t result;
+
+    config.key = MPU_SLAVE_RANGE_CHECK;
+    config.len = 3;
+    config.apply = TRUE;
+    config.data = data;
+
+    result = inv_mpu_get_compass_config(inv_get_dl_config(),
+                                        inv_get_serial_handle(),
+                                        inv_get_serial_handle(), &config);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    if(data[0] || data[1] || data[2]) {
+        /* some value clipped */
+        return INV_ERROR_COMPASS_DATA_ERROR;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/60xx/mlsdk/mllite/compass.h b/60xx/mlsdk/mllite/compass.h
new file mode 100644
index 0000000..f0bdb58
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/dmpDefault.c b/60xx/mlsdk/mllite/dmpDefault.c
new file mode 100644
index 0000000..6620d14
--- /dev/null
+++ b/60xx/mlsdk/mllite/dmpDefault.c
@@ -0,0 +1,417 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/***************************************************************************** *
+ * $Id: dmpDefault.c 5627 2011-06-10 22:34:18Z nroyer $ 
+ ******************************************************************************/
+
+/* WARNING: autogenerated code, do not modify */
+/**
+ *  @defgroup DMPDEFAULT 
+ *  @brief    Data and configuration for MLDmpDefaultOpen.
+ *
+ *  @{
+ *      @file   inv_setup_dmp.c
+ *      @brief  Data and configuration for MLDmpDefaultOpen.
+ */
+
+#include "mltypes.h"
+#include "dmpDefault.h"
+#include "dmpKey.h"
+#include "dmpmap.h"
+#include "ml.h"
+#include "mpu.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+
+#define CFG_25  703
+#define CFG_24  699
+#define CFG_26  707
+#define CFG_21  802
+#define CFG_20  645
+#define CFG_23  814
+#define CFG_TAP4  808
+#define CFG_TAP5  809
+#define CFG_TAP6  810
+#define CFG_1  783
+#define CFG_TAP0  802
+#define CFG_TAP1  804
+#define CFG_TAP2  805
+#define CFG_TAP3  806
+#define FCFG_AZ  878
+#define CFG_ORIENT_IRQ_1  715
+#define CFG_ORIENT_IRQ_2  738
+#define CFG_ORIENT_IRQ_3  743
+#define CFG_TAP_QUANTIZE  647
+#define FCFG_3  936
+#define CFG_TAP_CLEAR_STICKY  817
+#define FCFG_1  868
+#define CFG_ACCEL_FILTER  968
+#define FCFG_2  872
+#define CFG_3D  521
+#define CFG_3B  517
+#define CFG_3C  519
+#define FCFG_5  942
+#define FCFG_4  857
+#define FCFG_FSCALE  877
+#define CFG_TAP_JERK  639
+#define FCFG_6  996
+#define CFG_12  797
+#define FCFG_7  930
+#define CFG_14  790
+#define CFG_15  790
+#define CFG_16  815
+#define CFG_18  551
+#define CFG_6  823
+#define CFG_7  564
+#define CFG_4  526
+#define CFG_5  749
+#define CFG_3  515
+#define CFG_GYRO_SOURCE  777
+#define CFG_8  772
+#define CFG_9  778
+#define CFG_ORIENT_2  733
+#define CFG_ORIENT_1  713
+#define FCFG_ACCEL_INPUT  904
+#define CFG_TAP7  811
+#define CFG_TAP_SAVE_ACCB  687
+#define FCFG_ACCEL_INIT  831
+
+
+#define D_0_22  (22)
+#define D_0_24  (24)
+#define D_0_36  (36)
+#define D_0_52  (52)
+#define D_0_96  (96)
+#define D_0_104 (104)
+#define D_0_108 (108)
+#define D_0_163 (163)
+#define D_0_188 (188)
+#define D_0_192 (192)
+#define D_0_224 (224)
+#define D_0_228 (228)
+#define D_0_232 (232)
+#define D_0_236 (236)
+
+#define D_1_2 (256 + 2)
+#define D_1_4 (256 + 4)
+#define D_1_8 (256 + 8)
+#define D_1_10 (256 + 10)
+#define D_1_24 (256 + 24)
+#define D_1_28 (256 + 28)
+#define D_1_92 (256 + 92)
+#define D_1_96 (256 + 96)
+#define D_1_98 (256 + 98)
+#define D_1_106 (256 + 106)
+#define D_1_108 (256 + 108)
+#define D_1_112 (256 + 112)
+#define D_1_128 (256 + 144)
+#define D_1_152 (256 + 12)
+#define D_1_168 (256 + 168)
+#define D_1_175 (256 + 175)
+#define D_1_178 (256 + 178)
+#define D_1_236 (256 + 236)
+#define D_1_244 (256 + 244)
+
+
+static const tKeyLabel dmpTConfig[] = {
+    {KEY_CFG_25, CFG_25},
+    {KEY_CFG_24, CFG_24},
+    {KEY_CFG_26, CFG_26},
+    {KEY_CFG_21, CFG_21},
+    {KEY_CFG_20, CFG_20},
+    {KEY_CFG_23, CFG_23},
+    {KEY_CFG_TAP4, CFG_TAP4},
+    {KEY_CFG_TAP5, CFG_TAP5},
+    {KEY_CFG_TAP6, CFG_TAP6},
+    {KEY_CFG_1, CFG_1},
+    {KEY_CFG_TAP0, CFG_TAP0},
+    {KEY_CFG_TAP1, CFG_TAP1},
+    {KEY_CFG_TAP2, CFG_TAP2},
+    {KEY_CFG_TAP3, CFG_TAP3},
+    {KEY_FCFG_AZ, FCFG_AZ},
+    {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1},
+    {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2},
+    {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3},
+    {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE},
+    {KEY_FCFG_3, FCFG_3},
+    {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY},
+    {KEY_FCFG_1, FCFG_1},
+    //{KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER},
+    {KEY_FCFG_2, FCFG_2},
+    {KEY_CFG_3D, CFG_3D},
+    {KEY_CFG_3B, CFG_3B},
+    {KEY_CFG_3C, CFG_3C},
+    {KEY_FCFG_5, FCFG_5},
+    {KEY_FCFG_4, FCFG_4},
+    {KEY_FCFG_FSCALE, FCFG_FSCALE},
+    {KEY_CFG_TAP_JERK, CFG_TAP_JERK},
+    {KEY_FCFG_6, FCFG_6},
+    {KEY_CFG_12, CFG_12},
+    {KEY_FCFG_7, FCFG_7},
+    {KEY_CFG_14, CFG_14},
+    {KEY_CFG_15, CFG_15},
+    {KEY_CFG_16, CFG_16},
+    {KEY_CFG_18, CFG_18},
+    {KEY_CFG_6, CFG_6},
+    {KEY_CFG_7, CFG_7},
+    {KEY_CFG_4, CFG_4},
+    {KEY_CFG_5, CFG_5},
+    {KEY_CFG_3, CFG_3},
+    {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE},
+    {KEY_CFG_8, CFG_8},
+    {KEY_CFG_9, CFG_9},
+    {KEY_CFG_ORIENT_2, CFG_ORIENT_2},
+    {KEY_CFG_ORIENT_1, CFG_ORIENT_1},
+    {KEY_FCFG_ACCEL_INPUT, FCFG_ACCEL_INPUT},
+    {KEY_CFG_TAP7, CFG_TAP7},
+    {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB},
+    {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT},
+
+    {KEY_D_0_22, D_0_22},
+    {KEY_D_0_24, D_0_24},
+    {KEY_D_0_36, D_0_36},
+    {KEY_D_0_52, D_0_52},
+    {KEY_D_0_96, D_0_96},
+    {KEY_D_0_104, D_0_104},
+    {KEY_D_0_108, D_0_108},
+    {KEY_D_0_163, D_0_163},
+    {KEY_D_0_188, D_0_188},
+    {KEY_D_0_192, D_0_192},
+    {KEY_D_0_224, D_0_224},
+    {KEY_D_0_228, D_0_228},
+    {KEY_D_0_232, D_0_232},
+    {KEY_D_0_236, D_0_236},
+
+    {KEY_DMP_PREVPTAT, DMP_PREVPTAT},
+    {KEY_D_1_2, D_1_2},
+    {KEY_D_1_4, D_1_4},
+    {KEY_D_1_8, D_1_8},
+    {KEY_D_1_10, D_1_10},
+    {KEY_D_1_24, D_1_24},
+    {KEY_D_1_28, D_1_28},
+    {KEY_D_1_92, D_1_92},
+    {KEY_D_1_96, D_1_96},
+    {KEY_D_1_98, D_1_98},
+    {KEY_D_1_106, D_1_106},
+    {KEY_D_1_108, D_1_108},
+    {KEY_D_1_112, D_1_112},
+    {KEY_D_1_128, D_1_128},
+    {KEY_D_1_152, D_1_152},
+    {KEY_D_1_168, D_1_168},
+    {KEY_D_1_175, D_1_175},
+    {KEY_D_1_178, D_1_178},
+    {KEY_D_1_236, D_1_236},
+    {KEY_D_1_244, D_1_244},
+
+    {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN},
+    {KEY_DMP_TAP_THR_X, DMP_TAP_THX},
+    {KEY_DMP_TAP_THR_Y, DMP_TAP_THY},
+    {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ},
+    {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y},
+    {KEY_DMP_SH_TH_X, DMP_SH_TH_X},
+    {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z},
+    {KEY_DMP_ORIENT, DMP_ORIENT}
+};
+
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+static const unsigned short sConfig = 0x013f;
+#define SCD (1024)
+static const unsigned char dmpMemory[SCD] = {
+    0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x5a, 0xd6, 0x96, 0x06, 0x3f, 0xa3, 0x00, 0x00,
+    0x20, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x77, 0x8e, 0x00, 0x01, 0x00, 0x01,
+    0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01,
+    0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x03, 0x06, 0x00, 0x00, 0x05, 0x01, 0xe9, 0xa2, 0x0f,
+    0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x3e, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
+    0xfc, 0xba, 0xfa, 0x00, 0x01, 0x00, 0x80, 0x00, 0x02, 0x01, 0x80, 0x00, 0x03, 0x02, 0x80, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
+    0x00, 0x7d, 0x32, 0xba, 0x00, 0x0a, 0x1e, 0xd1, 0x00, 0x3a, 0xe8, 0x25, 0x00, 0x00, 0x00, 0x00,
+    0x3f, 0xd7, 0x96, 0x08, 0xff, 0xb3, 0x39, 0xf5, 0xfe, 0x11, 0x1b, 0x62, 0xfb, 0xf4, 0xb4, 0x52,
+    0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc,
+    0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4,
+    0x0d, 0x68, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10,
+
+    0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8,
+    0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x12, 0x82, 0x2d, 0x90,
+    0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0xff, 0xff, 0x00, 0x05, 0x02, 0x00, 0x00, 0x0c,
+    0x00, 0x03, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0xff, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00,
+    0xff, 0xec, 0x3f, 0xc8, 0xff, 0xee, 0x00, 0x00, 0xff, 0xfe, 0x40, 0x00, 0xff, 0xff, 0xff, 0xc8,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0xff, 0xff, 0xff, 0xff, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x03, 0x65, 0x00, 0x00, 0x00, 0x99, 0x00, 0x00, 0x02, 0xf5,
+
+    0x9e, 0xc5, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 0xc1, 0x83, 0x06, 0x26,
+    0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xc4, 0xad, 0x00, 0x2c, 0x54, 0x7c, 0xf9, 0xc5, 0xa3,
+    0xc1, 0xc3, 0x8f, 0x96, 0x19, 0xa6, 0x81, 0xda, 0x0c, 0xd9, 0x2e, 0xd8, 0xa3, 0x86, 0x31, 0x81,
+    0xa6, 0xd9, 0x30, 0x26, 0xd8, 0xd8, 0xfa, 0xc1, 0x8c, 0xc2, 0x99, 0xc5, 0xa3, 0x2d, 0x55, 0x7d,
+    0x81, 0x91, 0xac, 0x38, 0xad, 0x3a, 0xc3, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9,
+    0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9,
+    0x04, 0xae, 0xd8, 0x79, 0xd9, 0x04, 0xd8, 0x81, 0xfb, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81,
+    0xad, 0xd9, 0x01, 0xd8, 0xfa, 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad,
+    0xad, 0xad, 0xad, 0xfb, 0x2a, 0xd8, 0xd8, 0xf9, 0xc0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xfb,
+    0xac, 0x2e, 0x2e, 0xf9, 0xc1, 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30,
+    0x18, 0xa8, 0x98, 0x81, 0x28, 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xfa,
+    0xc0, 0x89, 0xac, 0x91, 0x2c, 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8,
+    0xd8, 0x79, 0xd9, 0xd8, 0xd8, 0xf9, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9,
+    0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xc1, 0x83, 0x93, 0x35, 0x3d, 0x80,
+    0x25, 0xda, 0xd8, 0xd8, 0x85, 0x69, 0xda, 0xd8, 0xd8, 0xf9, 0xc2, 0x93, 0x81, 0xa3, 0x28, 0x34,
+    0x3c, 0xfb, 0x91, 0xab, 0x8b, 0x18, 0xa3, 0x09, 0xd9, 0xab, 0x97, 0x0a, 0x91, 0x3c, 0xc0, 0x87,
+
+    0x9c, 0xc5, 0xa3, 0xdd, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x95, 0xf9, 0xa3, 0xa3, 0xa3, 0x9d, 0xf9,
+    0xa3, 0xa3, 0xa3, 0xa3, 0xf9, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, 0x91, 0xc3, 0x99, 0xf9, 0xa3, 0xa3,
+    0xa3, 0x98, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xfb, 0x9b, 0xa3, 0xa3,
+    0xdc, 0xc5, 0xa7, 0xf9, 0x26, 0x26, 0x26, 0xd8, 0xd8, 0xff, 0xd8, 0xd8, 0xd8, 0xd8, 0xd8, 0xc1,
+    0xc2, 0xc4, 0x81, 0xa0, 0x90, 0xfa, 0x2c, 0x80, 0x74, 0xfb, 0x70, 0xfa, 0x7c, 0xc0, 0x86, 0x98,
+    0xa8, 0xf9, 0xc9, 0x88, 0xa1, 0xfa, 0x0e, 0x97, 0x80, 0xf9, 0xa9, 0x2e, 0x2e, 0x2e, 0xaa, 0x2e,
+    0x2e, 0x2e, 0xfa, 0xaa, 0xc9, 0x2c, 0xcb, 0xa9, 0x4c, 0xcd, 0x6c, 0xf9, 0x89, 0xa5, 0xca, 0xcd,
+    0xcf, 0xc3, 0x9e, 0xa9, 0x3e, 0x5e, 0x7e, 0x85, 0xa5, 0x1a, 0x3e, 0x5e, 0xc2, 0xa5, 0x99, 0xfb,
+    0x08, 0x34, 0x5c, 0xf9, 0xa9, 0xc9, 0xcb, 0xcd, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97,
+    0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0xa9,
+    0xf9, 0x89, 0x26, 0x46, 0x66, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xaa, 0x98, 0x82, 0x87, 0x2d,
+    0x35, 0x3d, 0xc5, 0xa3, 0xc2, 0xc1, 0x97, 0x80, 0x4a, 0x4e, 0x4e, 0xa3, 0xfa, 0x48, 0xcd, 0xc9,
+    0xf9, 0xc4, 0xa9, 0x99, 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, 0xc3, 0x93,
+    0xa3, 0x0e, 0x16, 0x1e, 0xa9, 0x2c, 0x54, 0x7c, 0xc0, 0xc2, 0x83, 0x97, 0xaf, 0x08, 0xc4, 0xa8,
+    0x11, 0xc1, 0x8f, 0xc5, 0xaf, 0x98, 0xf8, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf9, 0xa3, 0x29,
+    0x55, 0x7d, 0xaf, 0x83, 0xc3, 0x93, 0xaf, 0xf8, 0x00, 0x28, 0x50, 0xc4, 0xc2, 0xc0, 0xf9, 0x97,
+};
+static tKeyLabel keys[NUM_KEYS];
+
+static unsigned short inv_setup_dmpGetAddress(unsigned short key)
+{
+    static int isSorted = 0;
+    if ( !isSorted ) {
+        int kk;
+        for (kk=0; kk<NUM_KEYS; ++kk) {
+            keys[ kk ].addr = 0xffff;
+            keys[ kk ].key = kk;
+        }
+        for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) {
+            keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr;
+        }
+        isSorted = 1;
+    }
+    if ( key >= NUM_KEYS )
+        return 0xffff;
+    return keys[ key ].addr;
+}
+
+
+/**
+ *  @brief
+ *  @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t inv_setup_dmp(void)
+{
+    inv_error_t result;
+    inv_set_get_address( inv_setup_dmpGetAddress );
+
+    result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_full_scale(2000.f);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_load_dmp(dmpMemory, SCD, sConfig);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_ignore_system_suspend(FALSE);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    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/60xx/mlsdk/mllite/dmpDefault.h b/60xx/mlsdk/mllite/dmpDefault.h
new file mode 100644
index 0000000..0670977
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/dmpDefaultMantis.c b/60xx/mlsdk/mllite/dmpDefaultMantis.c
new file mode 100644
index 0000000..f35aaca
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/dmpKey.h b/60xx/mlsdk/mllite/dmpKey.h
new file mode 100644
index 0000000..f152644
--- /dev/null
+++ b/60xx/mlsdk/mllite/dmpKey.h
@@ -0,0 +1,432 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+#ifndef DMPKEY_H__
+#define DMPKEY_H__
+
+#define KEY_CFG_25  0
+#define KEY_CFG_24                 (KEY_CFG_25+1)
+#define KEY_CFG_26                 (KEY_CFG_24+1)
+#define KEY_CFG_21                 (KEY_CFG_26+1)
+#define KEY_CFG_20                 (KEY_CFG_21+1)
+#define KEY_CFG_TAP4               (KEY_CFG_20+1)
+#define KEY_CFG_TAP5               (KEY_CFG_TAP4+1)
+#define KEY_CFG_TAP6               (KEY_CFG_TAP5+1)
+#define KEY_CFG_TAP7               (KEY_CFG_TAP6+1)
+#define KEY_CFG_TAP0               (KEY_CFG_TAP7+1)
+#define KEY_CFG_TAP1               (KEY_CFG_TAP0+1)
+#define KEY_CFG_TAP2               (KEY_CFG_TAP1+1)
+#define KEY_CFG_TAP3               (KEY_CFG_TAP2+1)
+#define KEY_CFG_TAP_QUANTIZE       (KEY_CFG_TAP3+1)
+#define KEY_CFG_TAP_JERK           (KEY_CFG_TAP_QUANTIZE+1)
+#define KEY_CFG_TAP_SAVE_ACCB      (KEY_CFG_TAP_JERK+1)
+#define KEY_CFG_TAP_CLEAR_STICKY   (KEY_CFG_TAP_SAVE_ACCB+1)
+#define KEY_FCFG_ACCEL_INPUT       (KEY_CFG_TAP_CLEAR_STICKY +1)
+#define KEY_FCFG_ACCEL_INIT        (KEY_FCFG_ACCEL_INPUT+1)
+#define KEY_CFG_23                 (KEY_FCFG_ACCEL_INIT+1)
+#define KEY_FCFG_1                 (KEY_CFG_23+1)
+#define KEY_FCFG_3                 (KEY_FCFG_1+1)
+#define KEY_FCFG_2                 (KEY_FCFG_3+1)
+#define KEY_CFG_3D                 (KEY_FCFG_2+1)
+#define KEY_CFG_3B                 (KEY_CFG_3D+1)
+#define KEY_CFG_3C                 (KEY_CFG_3B+1)
+#define KEY_FCFG_5                 (KEY_CFG_3C+1)
+#define KEY_FCFG_4                 (KEY_FCFG_5+1)
+#define KEY_FCFG_7                 (KEY_FCFG_4+1)
+#define KEY_FCFG_FSCALE            (KEY_FCFG_7+1)
+#define KEY_FCFG_AZ                (KEY_FCFG_FSCALE+1)
+#define KEY_FCFG_6                 (KEY_FCFG_AZ+1)
+#define KEY_FCFG_LSB4              (KEY_FCFG_6+1)
+#define KEY_CFG_12                 (KEY_FCFG_LSB4+1)
+#define KEY_CFG_14                 (KEY_CFG_12+1)
+#define KEY_CFG_15                 (KEY_CFG_14+1)
+#define KEY_CFG_16                 (KEY_CFG_15+1)
+#define KEY_CFG_18                 (KEY_CFG_16+1)
+#define KEY_CFG_6                  (KEY_CFG_18 + 1)
+#define KEY_CFG_7                  (KEY_CFG_6+1)
+#define KEY_CFG_4                  (KEY_CFG_7+1)
+#define KEY_CFG_5                  (KEY_CFG_4+1)
+#define KEY_CFG_2                  (KEY_CFG_5+1)
+#define KEY_CFG_3                  (KEY_CFG_2+1)
+#define KEY_CFG_1                  (KEY_CFG_3+1)
+#define KEY_CFG_EXTERNAL           (KEY_CFG_1+1)
+#define KEY_CFG_8                  (KEY_CFG_EXTERNAL+1)
+#define KEY_CFG_9                  (KEY_CFG_8+1)
+#define KEY_CFG_ORIENT_3           (KEY_CFG_9 + 1)
+#define KEY_CFG_ORIENT_2           (KEY_CFG_ORIENT_3 + 1)
+#define KEY_CFG_ORIENT_1           (KEY_CFG_ORIENT_2 + 1)
+#define KEY_CFG_GYRO_SOURCE        (KEY_CFG_ORIENT_1 + 1)
+#define KEY_CFG_ORIENT_IRQ_1       (KEY_CFG_GYRO_SOURCE + 1)
+#define KEY_CFG_ORIENT_IRQ_2       (KEY_CFG_ORIENT_IRQ_1 + 1)
+#define KEY_CFG_ORIENT_IRQ_3       (KEY_CFG_ORIENT_IRQ_2 + 1)
+#define KEY_FCFG_MAG_VAL           (KEY_CFG_ORIENT_IRQ_3 + 1)
+#define KEY_FCFG_MAG_MOV           (KEY_FCFG_MAG_VAL + 1)
+
+#define KEY_D_0_22                 (KEY_FCFG_MAG_MOV + 1)
+#define KEY_D_0_24                 (KEY_D_0_22+1)
+#define KEY_D_0_36                 (KEY_D_0_24+1)
+#define KEY_D_0_52                 (KEY_D_0_36+1)
+#define KEY_D_0_96                 (KEY_D_0_52+1)
+#define KEY_D_0_104                (KEY_D_0_96+1)
+#define KEY_D_0_108                (KEY_D_0_104+1)
+#define KEY_D_0_163                (KEY_D_0_108+1)
+#define KEY_D_0_188                (KEY_D_0_163+1)
+#define KEY_D_0_192                (KEY_D_0_188+1)
+#define KEY_D_0_224                (KEY_D_0_192+1)
+#define KEY_D_0_228                (KEY_D_0_224+1)
+#define KEY_D_0_232                (KEY_D_0_228+1)
+#define KEY_D_0_236                (KEY_D_0_232+1)
+
+#define KEY_DMP_PREVPTAT           (KEY_D_0_236+1)
+#define KEY_D_1_2                  (KEY_DMP_PREVPTAT+1)
+#define KEY_D_1_4                  (KEY_D_1_2 + 1)
+#define KEY_D_1_8                  (KEY_D_1_4 + 1)
+#define KEY_D_1_10                 (KEY_D_1_8+1)
+#define KEY_D_1_24                 (KEY_D_1_10+1)
+#define KEY_D_1_28                 (KEY_D_1_24+1)
+#define KEY_D_1_92                 (KEY_D_1_28+1)
+#define KEY_D_1_96                 (KEY_D_1_92+1)
+#define KEY_D_1_98                 (KEY_D_1_96+1)
+#define KEY_D_1_106                (KEY_D_1_98+1)
+#define KEY_D_1_108                (KEY_D_1_106+1)
+#define KEY_D_1_112                (KEY_D_1_108+1)
+#define KEY_D_1_128                (KEY_D_1_112+1)
+#define KEY_D_1_152                (KEY_D_1_128+1)
+#define KEY_D_1_168                (KEY_D_1_152+1)
+#define KEY_D_1_175                (KEY_D_1_168+1)
+#define KEY_D_1_178                (KEY_D_1_175+1)
+#define KEY_D_1_179                (KEY_D_1_178+1)
+#define KEY_D_1_236                (KEY_D_1_179+1)
+#define KEY_D_1_244                (KEY_D_1_236+1)
+#define KEY_D_2_12                 (KEY_D_1_244+1)
+#define KEY_D_2_96                 (KEY_D_2_12+1)
+#define KEY_D_2_108                (KEY_D_2_96+1)
+#define KEY_D_2_244                (KEY_D_2_108+1)
+#define KEY_D_2_248                (KEY_D_2_244+1)
+#define KEY_D_2_252                (KEY_D_2_248+1)
+
+// Compass Keys
+#define KEY_CPASS_BIAS_X            (KEY_D_2_252+1)
+#define KEY_CPASS_BIAS_Y            (KEY_CPASS_BIAS_X+1)
+#define KEY_CPASS_BIAS_Z            (KEY_CPASS_BIAS_Y+1)
+#define KEY_CPASS_MTX_00            (KEY_CPASS_BIAS_Z+1)
+#define KEY_CPASS_MTX_01            (KEY_CPASS_MTX_00+1)
+#define KEY_CPASS_MTX_02            (KEY_CPASS_MTX_01+1)
+#define KEY_CPASS_MTX_10            (KEY_CPASS_MTX_02+1)
+#define KEY_CPASS_MTX_11            (KEY_CPASS_MTX_10+1)
+#define KEY_CPASS_MTX_12            (KEY_CPASS_MTX_11+1)
+#define KEY_CPASS_MTX_20            (KEY_CPASS_MTX_12+1)
+#define KEY_CPASS_MTX_21            (KEY_CPASS_MTX_20+1)
+#define KEY_CPASS_MTX_22            (KEY_CPASS_MTX_21+1)
+
+// Mantis Keys
+#define KEY_CFG_MOTION_BIAS         (KEY_CPASS_MTX_22+1)
+
+#define KEY_DMP_TAPW_MIN           (KEY_CFG_MOTION_BIAS+1)
+#define KEY_DMP_TAP_THR_X          (KEY_DMP_TAPW_MIN+1)
+#define KEY_DMP_TAP_THR_Y          (KEY_DMP_TAP_THR_X+1)
+#define KEY_DMP_TAP_THR_Z          (KEY_DMP_TAP_THR_Y+1)
+#define KEY_DMP_SH_TH_Y            (KEY_DMP_TAP_THR_Z+1)
+#define KEY_DMP_SH_TH_X            (KEY_DMP_SH_TH_Y+1)
+#define KEY_DMP_SH_TH_Z            (KEY_DMP_SH_TH_X+1)
+#define KEY_DMP_ORIENT             (KEY_DMP_SH_TH_Z+1)
+#define KEY_D_ACT0                 (KEY_DMP_ORIENT+1)
+#define KEY_D_ACSX                 (KEY_D_ACT0+1)
+#define KEY_D_ACSY                 (KEY_D_ACSX+1)
+#define KEY_D_ACSZ                 (KEY_D_ACSY+1)
+
+// Pedometer Standalone only keys
+#define KEY_D_PEDSTD_BP_B          (KEY_D_ACSZ+1)
+#define KEY_D_PEDSTD_HP_A          (KEY_D_PEDSTD_BP_B+1)
+#define KEY_D_PEDSTD_HP_B          (KEY_D_PEDSTD_HP_A+1)
+#define KEY_D_PEDSTD_BP_A4         (KEY_D_PEDSTD_HP_B+1)
+#define KEY_D_PEDSTD_BP_A3         (KEY_D_PEDSTD_BP_A4+1)
+#define KEY_D_PEDSTD_BP_A2         (KEY_D_PEDSTD_BP_A3+1)
+#define KEY_D_PEDSTD_BP_A1         (KEY_D_PEDSTD_BP_A2+1)
+#define KEY_D_PEDSTD_INT_THRSH     (KEY_D_PEDSTD_BP_A1+1)
+#define KEY_D_PEDSTD_CLIP          (KEY_D_PEDSTD_INT_THRSH+1)
+#define KEY_D_PEDSTD_SB            (KEY_D_PEDSTD_CLIP+1)
+#define KEY_D_PEDSTD_SB_TIME       (KEY_D_PEDSTD_SB+1)
+#define KEY_D_PEDSTD_PEAKTHRSH     (KEY_D_PEDSTD_SB_TIME+1)
+#define KEY_D_PEDSTD_TIML          (KEY_D_PEDSTD_PEAKTHRSH+1)
+#define KEY_D_PEDSTD_TIMH          (KEY_D_PEDSTD_TIML+1)
+#define KEY_D_PEDSTD_PEAK          (KEY_D_PEDSTD_TIMH+1)
+#define KEY_D_PEDSTD_TIMECTR       (KEY_D_PEDSTD_PEAK+1)
+#define KEY_D_PEDSTD_STEPCTR       (KEY_D_PEDSTD_TIMECTR+1)
+#define KEY_D_PEDSTD_WALKTIME      (KEY_D_PEDSTD_STEPCTR+1)
+
+// EIS Keys
+#define KEY_P_EIS_FIFO_FOOTER      (KEY_D_PEDSTD_WALKTIME+1)
+#define KEY_P_EIS_FIFO_YSHIFT      (KEY_P_EIS_FIFO_FOOTER+1)
+#define KEY_P_EIS_DATA_RATE        (KEY_P_EIS_FIFO_YSHIFT+1)
+#define KEY_P_EIS_FIFO_XSHIFT      (KEY_P_EIS_DATA_RATE+1)
+#define KEY_P_EIS_FIFO_SYNC        (KEY_P_EIS_FIFO_XSHIFT+1)
+#define KEY_P_EIS_FIFO_ZSHIFT      (KEY_P_EIS_FIFO_SYNC+1)
+#define KEY_P_EIS_FIFO_READY       (KEY_P_EIS_FIFO_ZSHIFT+1)
+#define KEY_DMP_FOOTER             (KEY_P_EIS_FIFO_READY+1)
+#define KEY_DMP_INTX_HC            (KEY_DMP_FOOTER+1)
+#define KEY_DMP_INTX_PH            (KEY_DMP_INTX_HC+1)
+#define KEY_DMP_INTX_SH            (KEY_DMP_INTX_PH+1)
+#define KEY_DMP_AINV_SH            (KEY_DMP_INTX_SH +1)
+#define KEY_DMP_A_INV_XH           (KEY_DMP_AINV_SH+1)
+#define KEY_DMP_AINV_PH            (KEY_DMP_A_INV_XH+1)
+#define KEY_DMP_CTHX_H             (KEY_DMP_AINV_PH+1)
+#define KEY_DMP_CTHY_H             (KEY_DMP_CTHX_H+1)
+#define KEY_DMP_CTHZ_H             (KEY_DMP_CTHY_H+1)
+#define KEY_DMP_NCTHX_H            (KEY_DMP_CTHZ_H+1)
+#define KEY_DMP_NCTHY_H            (KEY_DMP_NCTHX_H+1)
+#define KEY_DMP_NCTHZ_H            (KEY_DMP_NCTHY_H+1)
+#define KEY_DMP_CTSQ_XH            (KEY_DMP_NCTHZ_H+1)
+#define KEY_DMP_CTSQ_YH            (KEY_DMP_CTSQ_XH+1)
+#define KEY_DMP_CTSQ_ZH            (KEY_DMP_CTSQ_YH+1)
+#define KEY_DMP_INTX_H             (KEY_DMP_CTSQ_ZH+1)
+#define KEY_DMP_INTY_H             (KEY_DMP_INTX_H+1)
+#define KEY_DMP_INTZ_H             (KEY_DMP_INTY_H+1)
+#define KEY_DMP_HPX_H              (KEY_DMP_INTZ_H+1)
+#define KEY_DMP_HPY_H              (KEY_DMP_HPX_H+1)
+#define KEY_DMP_HPZ_H              (KEY_DMP_HPY_H+1)
+
+// Stream Keys
+#define KEY_STREAM_P_GYRO_Z        (KEY_DMP_HPZ_H + 1)
+#define KEY_STREAM_P_GYRO_Y        (KEY_STREAM_P_GYRO_Z+1)
+#define KEY_STREAM_P_GYRO_X        (KEY_STREAM_P_GYRO_Y+1)
+#define KEY_STREAM_P_TEMP          (KEY_STREAM_P_GYRO_X+1)
+#define KEY_STREAM_P_AUX_Y         (KEY_STREAM_P_TEMP+1)
+#define KEY_STREAM_P_AUX_X         (KEY_STREAM_P_AUX_Y+1)
+#define KEY_STREAM_P_AUX_Z         (KEY_STREAM_P_AUX_X+1)
+#define KEY_STREAM_P_ACCEL_Y       (KEY_STREAM_P_AUX_Z+1)
+#define KEY_STREAM_P_ACCEL_X       (KEY_STREAM_P_ACCEL_Y+1)
+#define KEY_STREAM_P_FOOTER        (KEY_STREAM_P_ACCEL_X+1)
+#define KEY_STREAM_P_ACCEL_Z       (KEY_STREAM_P_FOOTER+1)
+
+#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z+1)
+
+    typedef struct {
+        unsigned short key;
+        unsigned short addr;
+    } tKeyLabel;
+    
+#define DINA0A 0x0a
+#define DINA22 0x22
+#define DINA42 0x42
+#define DINA5A 0x5a
+
+#define DINA06 0x06
+#define DINA0E 0x0e
+#define DINA16 0x16
+#define DINA1E 0x1e
+#define DINA26 0x26
+#define DINA2E 0x2e
+#define DINA36 0x36
+#define DINA3E 0x3e
+#define DINA46 0x46
+#define DINA4E 0x4e
+#define DINA56 0x56
+#define DINA5E 0x5e
+#define DINA66 0x66
+#define DINA6E 0x6e
+#define DINA76 0x76
+#define DINA7E 0x7e
+
+#define DINA00 0x00
+#define DINA08 0x08
+#define DINA10 0x10
+#define DINA18 0x18
+#define DINA20 0x20
+#define DINA28 0x28
+#define DINA30 0x30
+#define DINA38 0x38
+#define DINA40 0x40
+#define DINA48 0x48
+#define DINA50 0x50
+#define DINA58 0x58
+#define DINA60 0x60
+#define DINA68 0x68
+#define DINA70 0x70
+#define DINA78 0x78
+
+#define DINA04 0x04
+#define DINA0C 0x0c
+#define DINA14 0x14
+#define DINA1C 0x1C
+#define DINA24 0x24
+#define DINA2C 0x2c
+#define DINA34 0x34
+#define DINA3C 0x3c
+#define DINA44 0x44
+#define DINA4C 0x4c
+#define DINA54 0x54
+#define DINA5C 0x5c
+#define DINA64 0x64
+#define DINA6C 0x6c
+#define DINA74 0x74
+#define DINA7C 0x7c
+
+#define DINA01 0x01
+#define DINA09 0x09
+#define DINA11 0x11
+#define DINA19 0x19
+#define DINA21 0x21
+#define DINA29 0x29
+#define DINA31 0x31
+#define DINA39 0x39
+#define DINA41 0x41
+#define DINA49 0x49
+#define DINA51 0x51
+#define DINA59 0x59
+#define DINA61 0x61
+#define DINA69 0x69
+#define DINA71 0x71
+#define DINA79 0x79
+
+#define DINA25 0x25
+#define DINA2D 0x2d
+#define DINA35 0x35
+#define DINA3D 0x3d
+#define DINA4D 0x4d
+#define DINA55 0x55
+#define DINA5D 0x5D
+#define DINA6D 0x6d
+#define DINA75 0x75
+#define DINA7D 0x7d
+
+#define DINC00 0x00
+#define DINC01 0x01
+#define DINC02 0x02
+#define DINC03 0x03
+#define DINC08 0x08
+#define DINC09 0x09
+#define DINC0A 0x0a
+#define DINC0B 0x0b
+#define DINC10 0x10
+#define DINC11 0x11
+#define DINC12 0x12
+#define DINC13 0x13
+#define DINC18 0x18
+#define DINC19 0x19
+#define DINC1A 0x1a
+#define DINC1B 0x1b
+
+#define DINC20 0x20
+#define DINC21 0x21
+#define DINC22 0x22
+#define DINC23 0x23
+#define DINC28 0x28
+#define DINC29 0x29
+#define DINC2A 0x2a
+#define DINC2B 0x2b
+#define DINC30 0x30
+#define DINC31 0x31
+#define DINC32 0x32
+#define DINC33 0x33
+#define DINC38 0x38
+#define DINC39 0x39
+#define DINC3A 0x3a
+#define DINC3B 0x3b
+
+#define DINC40 0x40
+#define DINC41 0x41
+#define DINC42 0x42
+#define DINC43 0x43
+#define DINC48 0x48
+#define DINC49 0x49
+#define DINC4A 0x4a
+#define DINC4B 0x4b
+#define DINC50 0x50
+#define DINC51 0x51
+#define DINC52 0x52
+#define DINC53 0x53
+#define DINC58 0x58
+#define DINC59 0x59
+#define DINC5A 0x5a
+#define DINC5B 0x5b
+
+#define DINC60 0x60
+#define DINC61 0x61
+#define DINC62 0x62
+#define DINC63 0x63
+#define DINC68 0x68
+#define DINC69 0x69
+#define DINC6A 0x6a
+#define DINC6B 0x6b
+#define DINC70 0x70
+#define DINC71 0x71
+#define DINC72 0x72
+#define DINC73 0x73
+#define DINC78 0x78
+#define DINC79 0x79
+#define DINC7A 0x7a
+#define DINC7B 0x7b
+
+#if defined CONFIG_MPU_SENSORS_MPU3050
+#define DINA80 0x80
+#define DINA90 0x90
+#define DINAA0 0xa0
+#define DINAC9 0xc9
+#define DINACB 0xcb
+#define DINACD 0xcd
+#define DINACF 0xcf
+#define DINAC8 0xc8
+#define DINACA 0xca
+#define DINACC 0xcc
+#define DINACE 0xce
+#define DINAD8 0xd8
+#define DINADD 0xdd
+#define DINAF8 0xf8
+#define DINAFE 0xfe
+#define DINAC0 0xc0
+#define DINAC1 0xc1
+#define DINAC2 0xc2
+#define DINAC3 0xc3
+#define DINAC4 0xc4
+#define DINAC5 0xc5
+#elif defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+
+#define DINA80 0x80
+#define DINA90 0x90
+#define DINAA0 0xa0
+#define DINAC9 0xc9
+#define DINACB 0xcb
+#define DINACD 0xcd
+#define DINACF 0xcf
+#define DINAC8 0xc8
+#define DINACA 0xca
+#define DINACC 0xcc
+#define DINACE 0xce
+#define DINAD8 0xd8
+#define DINADD 0xdd
+#define DINAF8 0xf0
+#define DINAFE 0xfe
+
+#define DINBF8 0xf8
+#define DINAC0 0xb0
+#define DINAC1 0xb1
+#define DINAC2 0xb4
+#define DINAC3 0xb5
+#define DINAC4 0xb8
+#define DINAC5 0xb9
+#define DINBC0 0xc0
+#define DINBC2 0xc2
+#define DINBC4 0xc4
+#define DINBC6 0xc6
+#else
+#error No CONFIG_MPU_SENSORS_xxxx has been defined.
+#endif
+
+
+#endif // DMPKEY_H__
diff --git a/60xx/mlsdk/mllite/dmpconfig.txt b/60xx/mlsdk/mllite/dmpconfig.txt
new file mode 100644
index 0000000..4643ed5
--- /dev/null
+++ b/60xx/mlsdk/mllite/dmpconfig.txt
@@ -0,0 +1,3 @@
+major version    =  1
+minor version    =  0
+startAddr        =  0
diff --git a/60xx/mlsdk/mllite/dmpmap.h b/60xx/mlsdk/mllite/dmpmap.h
new file mode 100644
index 0000000..cb53c7c
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/invensense.h b/60xx/mlsdk/mllite/invensense.h
new file mode 100644
index 0000000..586dd25
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/ml.c b/60xx/mlsdk/mllite/ml.c
new file mode 100644
index 0000000..3328edd
--- /dev/null
+++ b/60xx/mlsdk/mllite/ml.c
@@ -0,0 +1,1885 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/******************************************************************************
+ *
+ * $Id: ml.c 5642 2011-06-14 02:26:20Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ *  @defgroup ML
+ *  @brief  Motion Library APIs.
+ *          The Motion Library processes gyroscopes, accelerometers, and
+ *          compasses to provide a physical model of the movement for the
+ *          sensors.
+ *          The results of this processing may be used to control objects
+ *          within a user interface environment, detect gestures, track 3D
+ *          movement for gaming applications, and analyze the blur created
+ *          due to hand movement while taking a picture.
+ *
+ *  @{
+ *      @file   ml.c
+ *      @brief  The Motion Library APIs.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+
+#include "ml.h"
+#include "mldl.h"
+#include "mltypes.h"
+#include "mlinclude.h"
+#include "compass.h"
+#include "dmpKey.h"
+#include "dmpDefault.h"
+#include "mlstates.h"
+#include "mlFIFO.h"
+#include "mlFIFOHW.h"
+#include "mlMathFunc.h"
+#include "mlsupervisor.h"
+#include "mlmath.h"
+#include "mlcontrol.h"
+#include "mldl_cfg.h"
+#include "mpu.h"
+#include "accel.h"
+#include "mlos.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include "mlBiasNoMotion.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-ml"
+
+#define ML_MOT_TYPE_NONE 0
+#define ML_MOT_TYPE_NO_MOTION 1
+#define ML_MOT_TYPE_MOTION_DETECTED 2
+
+#define ML_MOT_STATE_MOVING 0
+#define ML_MOT_STATE_NO_MOTION 1
+#define ML_MOT_STATE_BIAS_IN_PROG 2
+
+#define _mlDebug(x)             //{x}
+
+/* Global Variables */
+const unsigned char mlVer[] = { INV_VERSION };
+
+struct inv_params_obj inv_params_obj = {
+    INV_BIAS_UPDATE_FUNC_DEFAULT,   // bias_mode
+    INV_ORIENTATION_MASK_DEFAULT,   // orientation_mask
+    INV_PROCESSED_DATA_CALLBACK_DEFAULT,    // fifo_processed_func
+    INV_ORIENTATION_CALLBACK_DEFAULT,   // orientation_cb_func
+    INV_MOTION_CALLBACK_DEFAULT,    // motion_cb_func
+    INV_STATE_SERIAL_CLOSED     // starting state
+};
+
+struct inv_obj_t inv_obj;
+void *g_mlsl_handle;
+
+typedef struct {
+    // These describe callbacks happening everythime a DMP interrupt is processed
+    int_fast8_t numInterruptProcesses;
+    // Array of function pointers, function being void function taking void
+    inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES];
+
+} tMLXCallbackInterrupt;        // MLX_callback_t
+
+tMLXCallbackInterrupt mlxCallbackInterrupt;
+
+/* --------------- */
+/* -  Functions. - */
+/* --------------- */
+
+inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient);
+inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient);
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
+
+/**
+ *  @brief  Open serial connection with the MPU device.
+ *          This is the entry point of the MPL and must be
+ *          called prior to any other function call.
+ *
+ *  @param  port     System handle for 'port' MPU device is found on.
+ *                   The significance of this parameter varies by
+ *                   platform. It is passed as 'port' to MLSLSerialOpen.
+ *
+ *  @return INV_SUCCESS or error code.
+ */
+inv_error_t inv_serial_start(char const *port)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+
+    if (inv_get_state() >= INV_STATE_SERIAL_OPENED)
+        return INV_SUCCESS;
+
+    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    result = inv_serial_open(port, &g_mlsl_handle);
+    if (INV_SUCCESS != result) {
+        (void)inv_state_transition(INV_STATE_SERIAL_CLOSED);
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  Close the serial communication.
+ *          This function needs to be called explicitly to shut down
+ *          the communication with the device.  Calling inv_dmp_close()
+ *          won't affect the established serial communication.
+ *  @return INV_SUCCESS; non-zero error code otherwise.
+ */
+inv_error_t inv_serial_stop(void)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() == INV_STATE_SERIAL_CLOSED)
+        return INV_SUCCESS;
+
+    result = inv_state_transition(INV_STATE_SERIAL_CLOSED);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result);
+    }
+    result = inv_serial_close(g_mlsl_handle);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result);
+    }
+    return result;
+}
+
+/**
+ *  @brief  Get the serial file handle to the device.
+ *  @return The serial file handle.
+ */
+void *inv_get_serial_handle(void)
+{
+    INVENSENSE_FUNC_START;
+    return g_mlsl_handle;
+}
+
+/**
+ *  @brief  apply the choosen orientation and full scale range
+ *          for gyroscopes, accelerometer, and compass.
+ *  @return INV_SUCCESS if successful, a non-zero code otherwise.
+ */
+inv_error_t inv_apply_calibration(void)
+{
+    INVENSENSE_FUNC_START;
+    signed char gyroCal[9] = { 0 };
+    signed char accelCal[9] = { 0 };
+    signed char magCal[9] = { 0 };
+    float gyroScale = 2000.f;
+    float accelScale = 0.f;
+    float magScale = 0.f;
+
+    inv_error_t result;
+    int ii;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    for (ii = 0; ii < 9; ii++) {
+        gyroCal[ii] = mldl_cfg->pdata->orientation[ii];
+        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) */
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+    inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim;
+#else
+    inv_obj.accel_sens /= 2;
+#endif
+    }
+    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();
+#if defined CONFIG_MPU_SENSORS_MPU3050
+    regs[0] = DINAD8 + 2;
+    regs[1] = DINA0C;
+    regs[2] = DINAD8 + 1;
+    result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+#else
+#endif
+    regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
+    regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
+    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    memset(regs, 0, 8);
+    result = inv_set_mpu_memory(KEY_D_1_96, 8, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    result =
+        inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs));
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    inv_set_motion_state(INV_MOTION);
+    return result;
+}
+
+/**
+ * @internal
+ * @brief   inv_start_bias_calc starts the bias calculation on the MPU.
+ */
+void inv_start_bias_calc(void)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_obj.suspend = 1;
+}
+
+/**
+ * @internal
+ * @brief   inv_stop_bias_calc stops the bias calculation on the MPU.
+ */
+void inv_stop_bias_calc(void)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_obj.suspend = 0;
+}
+
+/**
+ *  @brief  inv_update_data fetches data from the fifo and updates the
+ *          motion algorithms.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start() must have been called.
+ *
+ *  @note   Motion algorithm data is constant between calls to inv_update_data
+ *
+ * @return
+ * - INV_SUCCESS
+ * - Non-zero error code
+ */
+inv_error_t inv_update_data(void)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+    int_fast8_t got, ftry;
+    uint_fast8_t mpu_interrupt;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (inv_get_state() != INV_STATE_DMP_STARTED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    // Set the maximum number of FIFO packets we want to process
+    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
+        ftry = 100;             // Large enough to process all packets
+    } else {
+        ftry = 1;
+    }
+
+    // Go and process at most ftry number of packets, probably less than ftry
+    result = inv_read_and_process_fifo(ftry, &got);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    // Process all interrupts
+    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1);
+    if (mpu_interrupt) {
+        inv_clear_interrupt_trigger(INTSRC_AUX1);
+    }
+    // Check if interrupt was from MPU
+    mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU);
+    if (mpu_interrupt) {
+        inv_clear_interrupt_trigger(INTSRC_MPU);
+    }
+    // Take care of the callbacks that want to be notified when there was an MPU interrupt
+    if (mpu_interrupt) {
+        inv_run_dmp_interupt_cb();
+    }
+
+    result = inv_get_fifo_status();
+    return result;
+}
+
+/**
+ *  @brief  inv_check_flag returns the value of a flag.
+ *          inv_check_flag can be used to check a number of flags,
+ *          allowing users to poll flags rather than register callback
+ *          functions. If a flag is set to True when inv_check_flag is called,
+ *          the flag is automatically reset.
+ *          The flags are:
+ *          - INV_RAW_DATA_READY
+ *          Indicates that new raw data is available.
+ *          - INV_PROCESSED_DATA_READY
+ *          Indicates that new processed data is available.
+ *          - INV_GOT_GESTURE
+ *          Indicates that a gesture has been detected by the gesture engine.
+ *          - INV_MOTION_STATE_CHANGE
+ *          Indicates that a change has been made from motion to no motion,
+ *          or vice versa.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start() must have been called.
+ *
+ *  @param flag The flag to check.
+ *
+ * @return TRUE or FALSE state of the flag
+ */
+int inv_check_flag(int flag)
+{
+    INVENSENSE_FUNC_START;
+    int flagReturn = inv_obj.flags[flag];
+
+    inv_obj.flags[flag] = 0;
+    return flagReturn;
+}
+
+/**
+ *  @brief  Enable generation of the DMP interrupt when Motion or no-motion
+ *          is detected
+ *  @param on
+ *          Boolean to turn the interrupt on or off.
+ *  @return INV_SUCCESS or non-zero error code.
+ */
+inv_error_t inv_set_motion_interrupt(unsigned char on)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    unsigned char regs[2] = { 0 };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (on) {
+        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        inv_obj.interrupt_sources |= INV_INT_MOTION;
+    } else {
+        inv_obj.interrupt_sources &= ~INV_INT_MOTION;
+        if (!inv_obj.interrupt_sources) {
+            result = inv_get_dl_cfg_int(0);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+    }
+
+    if (on) {
+        regs[0] = DINAFE;
+    } else {
+        regs[0] = DINAD8;
+    }
+    result = inv_set_mpu_memory(KEY_CFG_7, 1, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ * Enable generation of the DMP interrupt when a FIFO packet is ready
+ *
+ * @param on Boolean to turn the interrupt on or off
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t inv_set_fifo_interrupt(unsigned char on)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    unsigned char regs[2] = { 0 };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (on) {
+        result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        inv_obj.interrupt_sources |= INV_INT_FIFO;
+    } else {
+        inv_obj.interrupt_sources &= ~INV_INT_FIFO;
+        if (!inv_obj.interrupt_sources) {
+            result = inv_get_dl_cfg_int(0);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+    }
+
+    if (on) {
+        regs[0] = DINAFE;
+    } else {
+        regs[0] = DINAD8;
+    }
+    result = inv_set_mpu_memory(KEY_CFG_6, 1, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ * @brief   Get the current set of DMP interrupt sources.
+ *          These interrupts are generated by the DMP and can be
+ *          routed to the MPU interrupt line via internal
+ *          settings.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *
+ * @return  Currently enabled interrupt sources.  The possible interrupts are:
+ *          - INV_INT_FIFO,
+ *          - INV_INT_MOTION,
+ *          - INV_INT_TAP
+ */
+int inv_get_interrupts(void)
+{
+    INVENSENSE_FUNC_START;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return 0;               // error
+
+    return inv_obj.interrupt_sources;
+}
+
+/**
+ *  @brief  Sets up the Accelerometer calibration and scale factor.
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion Application
+ *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
+ *          offers a good coverage on the mounting matrices and explains how
+ *          to use them.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *  @pre    inv_dmp_start() must <b>NOT</b> have been called.
+ *
+ *  @see    inv_set_gyro_calibration().
+ *  @see    inv_set_compass_calibration().
+ *
+ *  @param[in]  range
+ *                  The range of the accelerometers in g's. An accelerometer
+ *                  that has a range of +2g's to -2g's should pass in 2.
+ *  @param[in]  orientation
+ *                  A 9 element matrix that represents how the accelerometers
+ *                  are oriented with respect to the device they are mounted
+ *                  in and the reference axis system.
+ *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
+ *                  This example corresponds to a 3 x 3 identity matrix.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_accel_calibration(float range, signed char *orientation)
+{
+    INVENSENSE_FUNC_START;
+    float cal[9];
+    float scale = range / 32768.f;
+    int kk;
+    unsigned long sf;
+    inv_error_t result;
+    unsigned char regs[4] = { 0, 0, 0, 0 };
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (inv_get_state() != INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    /* Apply zero g-offset values */
+    if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) {
+        regs[0] = 0x80;
+        regs[1] = 0x0;
+        regs[2] = 0x80;
+        regs[3] = 0x0;
+    }
+
+    if (inv_dmpkey_supported(KEY_D_1_152)) {
+        result = inv_set_mpu_memory(KEY_D_1_152, 4, &regs[0]);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    if (scale == 0) {
+        inv_obj.accel_sens = 0;
+    }
+
+    if (mldl_cfg->accel->id) {
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+        unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C };
+#else
+        unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
+#endif
+        struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+        unsigned char regs[3];
+        unsigned short orient;
+
+        for (kk = 0; kk < 9; ++kk) {
+            cal[kk] = scale * orientation[kk];
+            inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
+        }
+
+        orient = inv_orientation_matrix_to_scalar(orientation);
+        regs[0] = tmp[orient & 3];
+        regs[1] = tmp[(orient >> 3) & 3];
+        regs[2] = tmp[(orient >> 6) & 3];
+        result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        regs[0] = DINA26;
+        regs[1] = DINA46;
+#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
+        regs[2] = DINA66;
+#else
+        if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision)
+                == MPU_PRODUCT_KEY_B1_E1_5)
+            regs[2] = DINA76;
+        else
+            regs[2] = DINA66;
+#endif
+        if (orient & 4)
+            regs[0] |= 1;
+        if (orient & 0x20)
+            regs[1] |= 1;
+        if (orient & 0x100)
+            regs[2] |= 1;
+
+        result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) {
+            result = inv_freescale_sensor_fusion_16bit(orient);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) {
+            result = inv_freescale_sensor_fusion_8bit(orient);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+    }
+
+    if (inv_obj.accel_sens != 0) {
+        sf = (1073741824L / inv_obj.accel_sens);
+    } else {
+        sf = 0;
+    }
+    regs[0] = (unsigned char)((sf >> 8) & 0xff);
+    regs[1] = (unsigned char)(sf & 0xff);
+    result = inv_set_mpu_memory(KEY_D_0_108, 2, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  Sets up the Gyro calibration and scale factor.
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion Application
+ *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
+ *          offers a good coverage on the mounting matrices and explains
+ *          how to use them.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
+ *
+ *  @see    inv_set_accel_calibration().
+ *  @see    inv_set_compass_calibration().
+ *
+ *  @param[in]  range
+ *                  The range of the gyros in degrees per second. A gyro
+ *                  that has a range of +2000 dps to -2000 dps should pass in
+ *                  2000.
+ *  @param[in] orientation
+ *                  A 9 element matrix that represents how the gyro are oriented
+ *                  with respect to the device they are mounted in. A typical
+ *                  set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This
+ *                  example corresponds to a 3 x 3 identity matrix.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_gyro_calibration(float range, signed char *orientation)
+{
+    INVENSENSE_FUNC_START;
+
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    int kk;
+    float scale;
+    inv_error_t result;
+
+    unsigned char regs[12] = { 0 };
+    unsigned char maxVal = 0;
+    unsigned char tmpPtr = 0;
+    unsigned char tmpSign = 0;
+    unsigned char i = 0;
+    int sf = 0;
+
+    if (inv_get_state() != INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (mldl_cfg->gyro_sens_trim != 0) {
+        /* adjust the declared range to consider the gyro_sens_trim
+           of this part when different from the default (first dividend) */
+        range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim;
+    }
+
+    scale = range / 32768.f;    // inverse of sensitivity for the given full scale range
+    inv_obj.gyro_sens = (long)(range * 32768L);
+
+    for (kk = 0; kk < 9; ++kk) {
+        inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30));
+        inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30));
+    }
+
+    {
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+        unsigned char tmpD = DINA4C;
+        unsigned char tmpE = DINACD;
+        unsigned char tmpF = DINA6C;
+#else
+        unsigned char tmpD = DINAC9;
+        unsigned char tmpE = DINA2C;
+        unsigned char tmpF = DINACB;
+#endif
+        regs[3] = DINA36;
+        regs[4] = DINA56;
+        regs[5] = DINA76;
+
+        for (i = 0; i < 3; i++) {
+            maxVal = 0;
+            tmpSign = 0;
+            if (inv_obj.gyro_orient[0 + 3 * i] < 0)
+                tmpSign = 1;
+            if (ABS(inv_obj.gyro_orient[1 + 3 * i]) >
+                ABS(inv_obj.gyro_orient[0 + 3 * i])) {
+                maxVal = 1;
+                if (inv_obj.gyro_orient[1 + 3 * i] < 0)
+                    tmpSign = 1;
+            }
+            if (ABS(inv_obj.gyro_orient[2 + 3 * i]) >
+                ABS(inv_obj.gyro_orient[1 + 3 * i])) {
+                tmpSign = 0;
+                maxVal = 2;
+                if (inv_obj.gyro_orient[2 + 3 * i] < 0)
+                    tmpSign = 1;
+            }
+            if (maxVal == 0) {
+                regs[tmpPtr++] = tmpD;
+                if (tmpSign)
+                    regs[tmpPtr + 2] |= 0x01;
+            } else if (maxVal == 1) {
+                regs[tmpPtr++] = tmpE;
+                if (tmpSign)
+                    regs[tmpPtr + 2] |= 0x01;
+            } else {
+                regs[tmpPtr++] = tmpF;
+                if (tmpSign)
+                    regs[tmpPtr + 2] |= 0x01;
+            }
+        }
+        result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_set_mpu_memory(KEY_FCFG_3, 3, &regs[3]);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384
+        inv_obj.gyro_sf =
+            (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL);
+        result =
+            inv_set_mpu_memory(KEY_D_0_104, 4,
+                               inv_int32_to_big8(inv_obj.gyro_sf, regs));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        if (inv_obj.gyro_sens != 0) {
+            sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens);
+        } else {
+            sf = 0;
+        }
+
+        result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Sets up the Compass calibration and scale factor.
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion Application
+ *          Note" document provided.  Section 5, "Sensor Mounting Orientation"
+ *          offers a good coverage on the mounting matrices and explains
+ *          how to use them.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *  @pre    inv_dmp_start() must have <b>NOT</b> been called.
+ *
+ *  @see    inv_set_gyro_calibration().
+ *  @see    inv_set_accel_calibration().
+ *
+ *  @param[in] range
+ *                  The range of the compass.
+ *  @param[in] orientation
+ *                  A 9 element matrix that represents how the compass is
+ *                  oriented with respect to the device they are mounted in.
+ *                  A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
+ *                  This example corresponds to a 3 x 3 identity matrix.
+ *                  The matrix describes how to go from the chip mounting to
+ *                  the body of the device.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_compass_calibration(float range, signed char *orientation)
+{
+    INVENSENSE_FUNC_START;
+    float cal[9];
+    float scale = range / 32768.f;
+    int kk;
+    unsigned short compassId = 0;
+
+    compassId = inv_get_compass_id();
+
+    if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883)
+        || (compassId == COMPASS_ID_LSM303M)) {
+        scale /= 32.0f;
+    }
+
+    for (kk = 0; kk < 9; ++kk) {
+        cal[kk] = scale * orientation[kk];
+        inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
+    }
+
+    inv_obj.compass_sens = (long)(scale * 1073741824L);
+
+    if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) {
+        unsigned char reg0[4] = { 0, 0, 0, 0 };
+        unsigned char regp[4] = { 64, 0, 0, 0 };
+        unsigned char regn[4] = { 64 + 128, 0, 0, 0 };
+        unsigned char *reg;
+        int_fast8_t kk;
+        unsigned short keyList[9] =
+            { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02,
+            KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12,
+            KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22
+        };
+
+        for (kk = 0; kk < 9; ++kk) {
+
+            if (orientation[kk] == 1)
+                reg = regp;
+            else if (orientation[kk] == -1)
+                reg = regn;
+            else
+                reg = reg0;
+            inv_set_mpu_memory(keyList[kk], 4, reg);
+        }
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+* @internal
+* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup.
+*/
+inv_error_t inv_set_dead_zone(void)
+{
+    unsigned char reg;
+    inv_error_t result;
+    extern struct control_params cntrl_params;
+
+    if (cntrl_params.functions & INV_DEAD_ZONE) {
+        reg = 0x08;
+    } else {
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+        reg = 0;
+#else
+        if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
+            reg = 0x2;
+        } else {
+            reg = 0;
+        }
+#endif
+    }
+
+    result = inv_set_mpu_memory(KEY_D_0_163, 1, &reg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  inv_set_bias_update is used to register which algorithms will be
+ *          used to automatically reset the gyroscope bias.
+ *          The engine INV_BIAS_UPDATE must be enabled for these algorithms to
+ *          run.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start()
+ *          must <b>NOT</b> have been called.
+ *
+ *  @param  function    A function or bitwise OR of functions that determine
+ *                      how the gyroscope bias will be automatically updated.
+ *                      Functions include:
+ *                      - INV_NONE or 0,
+ *                      - INV_BIAS_FROM_NO_MOTION,
+ *                      - INV_BIAS_FROM_GRAVITY,
+ *                      - INV_BIAS_FROM_TEMPERATURE,
+                    \ifnot UMPL
+ *                      - INV_BIAS_FROM_LPF,
+ *                      - INV_MAG_BIAS_FROM_MOTION,
+ *                      - INV_MAG_BIAS_FROM_GYRO,
+ *                      - INV_ALL.
+ *                   \endif
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_bias_update(unsigned short function)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[4];
+    long tmp[3] = { 0, 0, 0 };
+    inv_error_t result = INV_SUCCESS;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (inv_get_state() != INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    /* do not allow progressive no motion bias tracker to run -
+       it's not fully debugged */
+    function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround
+    MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n");
+
+    // You must use EnableFastNoMotion() to get this feature
+    function &= ~INV_BIAS_FROM_FAST_NO_MOTION;
+
+    // You must use DisableFastNoMotion() to turn this feature off
+    if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION)
+        function |= INV_BIAS_FROM_FAST_NO_MOTION;
+
+    /*--- remove magnetic components from bias tracking
+          if there is no compass ---*/
+    if (!inv_compass_present()) {
+        function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION);
+    } else {
+        function &= ~(INV_BIAS_FROM_LPF);
+    }
+
+    // Enable function sets bias from no motion
+    inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION);
+
+    if (function & INV_BIAS_FROM_NO_MOTION) {
+        inv_enable_bias_no_motion();
+    } else {
+        inv_disable_bias_no_motion();
+    }
+
+    if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
+        regs[0] = DINA80 + 2;
+        regs[1] = DINA2D;
+        regs[2] = DINA55;
+        regs[3] = DINA7D;
+    } else {
+        regs[0] = DINA80 + 7;
+        regs[1] = DINA2D;
+        regs[2] = DINA35;
+        regs[3] = DINA3D;
+    }
+    result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_dead_zone();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) &&
+        !inv_compass_present()) {
+        result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    } else {
+        result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    inv_obj.factory_temp_comp = 0;  // FIXME, workaround
+    if ((mldl_cfg->offset_tc[0] != 0) ||
+        (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) {
+        inv_obj.factory_temp_comp = 1;
+    }
+
+    if (inv_obj.factory_temp_comp == 0) {
+        if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) {
+            result = inv_set_gyro_temp_slope(inv_obj.temp_slope);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        } else {
+            result = inv_set_gyro_temp_slope(tmp);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+    } else {
+        inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE;
+        MPL_LOGV("factory temperature compensation coefficients available - "
+                 "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n");
+    }
+
+    /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on
+           compass and accel data, is to have accelerometer data delivered in the
+           FIFO ----*/
+    if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY)
+         && inv_compass_present())
+        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO)
+        || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) {
+        inv_send_accel(INV_ALL, INV_32_BIT);
+        inv_send_gyro(INV_ALL, INV_32_BIT);
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_motion_state is used to determine if the device is in
+ *          a 'motion' or 'no motion' state.
+ *          inv_get_motion_state returns INV_MOTION of the device is moving,
+ *          or INV_NO_MOTION if the device is not moving.
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start()
+ *          must have been called.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+int inv_get_motion_state(void)
+{
+    INVENSENSE_FUNC_START;
+    return inv_obj.motion_state;
+}
+
+/**
+ *  @brief  inv_set_no_motion_thresh is used to set the threshold for
+ *          detecting INV_NO_MOTION
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *
+ *  @param  thresh  A threshold scaled in degrees per second.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_no_motion_thresh(float thresh)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned char regs[4] = { 0 };
+    long tmp;
+    INVENSENSE_FUNC_START;
+
+    tmp = (long)(thresh * thresh * 2.045f);
+    if (tmp < 0) {
+        return INV_ERROR;
+    } else if (tmp > 8180000L) {
+        return INV_ERROR;
+    }
+    inv_obj.no_motion_threshold = tmp;
+
+    regs[0] = (unsigned char)(tmp >> 24);
+    regs[1] = (unsigned char)((tmp >> 16) & 0xff);
+    regs[2] = (unsigned char)((tmp >> 8) & 0xff);
+    regs[3] = (unsigned char)(tmp & 0xff);
+
+    result = inv_set_mpu_memory(KEY_D_1_108, 4, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_reset_motion();
+    return result;
+}
+
+/**
+ *  @brief  inv_set_no_motion_threshAccel is used to set the threshold for
+ *          detecting INV_NO_MOTION with accelerometers when Gyros have
+ *          been turned off
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *
+ *  @param  thresh  A threshold in g's scaled by 2^32
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_no_motion_threshAccel(long thresh)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_obj.no_motion_accel_threshold = thresh;
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  inv_set_no_motion_time is used to set the time required for
+ *          detecting INV_NO_MOTION
+ *
+ *  @pre    inv_dmp_open()
+ *          @ifnot MPL_MF
+ *              or inv_open_low_power_pedometer()
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          must have been called.
+ *
+ *  @param  time    A time in seconds.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_no_motion_time(float time)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned char regs[2] = { 0 };
+    long tmp;
+
+    INVENSENSE_FUNC_START;
+
+    tmp = (long)(time * 200);
+    if (tmp < 0) {
+        return INV_ERROR;
+    } else if (tmp > 65535L) {
+        return INV_ERROR;
+    }
+    inv_obj.motion_duration = (unsigned short)tmp;
+
+    regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
+    regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
+    result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_reset_motion();
+    return result;
+}
+
+/**
+ *  @brief  inv_get_version is used to get the ML version.
+ *
+ *  @pre    inv_get_version can be called at any time.
+ *
+ *  @param  version     inv_get_version writes the ML version
+ *                      string pointer to version.
+ *
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_get_version(unsigned char **version)
+{
+    INVENSENSE_FUNC_START;
+
+    *version = (unsigned char *)mlVer;  //fixme we are wiping const
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief Check for the presence of the gyro sensor.
+ *
+ * This is not a physical check but a logical check and the value can change
+ * dynamically based on calls to inv_set_mpu_sensors().
+ *
+ * @return  TRUE if the gyro is enabled FALSE otherwise.
+ */
+int inv_get_gyro_present(void)
+{
+    return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO |
+                                                     INV_Z_GYRO);
+}
+
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;                  // error
+    return b;
+}
+
+unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
+{
+    unsigned short scalar;
+    /*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+     */
+
+    scalar = inv_row_2_scale(mtx);
+    scalar |= inv_row_2_scale(mtx + 3) << 3;
+    scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+    return scalar;
+}
+
+/* Setups up the Freescale 16-bit accel for Sensor Fusion
+* @param[in] orient A scalar representation of the orientation
+*  that describes how to go from the Chip Orientation
+*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
+*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
+*/
+inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient)
+{
+    unsigned char rr[3];
+    inv_error_t result;
+
+    orient = orient & 0xdb;
+    switch (orient) {
+    default:
+        // Typically 0x88
+        rr[0] = DINACC;
+        rr[1] = DINACF;
+        rr[2] = DINA0E;
+        break;
+    case 0x50:
+        rr[0] = DINACE;
+        rr[1] = DINA0E;
+        rr[2] = DINACD;
+        break;
+    case 0x81:
+        rr[0] = DINACE;
+        rr[1] = DINACB;
+        rr[2] = DINA0E;
+        break;
+    case 0x11:
+        rr[0] = DINACC;
+        rr[1] = DINA0E;
+        rr[2] = DINACB;
+        break;
+    case 0x42:
+        rr[0] = DINA0A;
+        rr[1] = DINACF;
+        rr[2] = DINACB;
+        break;
+    case 0x0a:
+        rr[0] = DINA0A;
+        rr[1] = DINACB;
+        rr[2] = DINACD;
+        break;
+    }
+    result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr);
+    return result;
+}
+
+/* Setups up the Freescale 8-bit accel for Sensor Fusion
+* @param[in] orient A scalar representation of the orientation
+*  that describes how to go from the Chip Orientation
+*  to the Board Orientation often times called the Body Orientation in Invensense Documentation.
+*  inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
+*/
+inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient)
+{
+    unsigned char regs[27];
+    inv_error_t result;
+    uint_fast8_t kk;
+
+    orient = orient & 0xdb;
+    kk = 0;
+
+    regs[kk++] = DINAC3;
+    regs[kk++] = DINA90 + 14;
+    regs[kk++] = DINAA0 + 9;
+    regs[kk++] = DINA3E;
+    regs[kk++] = DINA5E;
+    regs[kk++] = DINA7E;
+
+    regs[kk++] = DINAC2;
+    regs[kk++] = DINAA0 + 9;
+    regs[kk++] = DINA90 + 9;
+    regs[kk++] = DINAF8 + 2;
+
+    switch (orient) {
+    default:
+        // Typically 0x88
+        regs[kk++] = DINACB;
+
+        regs[kk++] = DINA54;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+
+        regs[kk++] = DINACD;
+        break;
+    case 0x50:
+        regs[kk++] = DINACB;
+
+        regs[kk++] = DINACF;
+
+        regs[kk++] = DINA7C;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        break;
+    case 0x81:
+        regs[kk++] = DINA2C;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+
+        regs[kk++] = DINACD;
+
+        regs[kk++] = DINACB;
+        break;
+    case 0x11:
+        regs[kk++] = DINA2C;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINA28;
+        regs[kk++] = DINACB;
+        regs[kk++] = DINACF;
+        break;
+    case 0x42:
+        regs[kk++] = DINACF;
+        regs[kk++] = DINACD;
+
+        regs[kk++] = DINA7C;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        regs[kk++] = DINA78;
+        break;
+    case 0x0a:
+        regs[kk++] = DINACD;
+
+        regs[kk++] = DINA54;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+        regs[kk++] = DINA50;
+
+        regs[kk++] = DINACF;
+        break;
+    }
+
+    regs[kk++] = DINA90 + 7;
+    regs[kk++] = DINAF8 + 3;
+    regs[kk++] = DINAA0 + 9;
+    regs[kk++] = DINA0E;
+    regs[kk++] = DINA0E;
+    regs[kk++] = DINA0E;
+
+    regs[kk++] = DINAF8 + 1;    // filler
+
+    result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/**
+ * Controlls each sensor and each axis when the motion processing unit is
+ * running.  When it is not running, simply records the state for later.
+ *
+ * NOTE: In this version only full sensors controll is allowed.  Independent
+ * axis control will return an error.
+ *
+ * @param sensors Bit field of each axis desired to be turned on or off
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t inv_set_mpu_sensors(unsigned long sensors)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char state = inv_get_state();
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    inv_error_t result = INV_SUCCESS;
+    unsigned short fifoRate;
+
+    if (state < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) &&
+        ((sensors & INV_THREE_AXIS_ACCEL) != 0)) {
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    }
+    if (((sensors & INV_THREE_AXIS_ACCEL) != 0) &&
+        (mldl_cfg->pdata->accel.get_slave_descr == 0)) {
+        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
+    }
+
+    if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) &&
+        ((sensors & INV_THREE_AXIS_COMPASS) != 0)) {
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    }
+    if (((sensors & INV_THREE_AXIS_COMPASS) != 0) &&
+        (mldl_cfg->pdata->compass.get_slave_descr == 0)) {
+        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
+    }
+
+    if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) &&
+        ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) {
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    }
+    if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) &&
+        (mldl_cfg->pdata->pressure.get_slave_descr == 0)) {
+        return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
+    }
+
+    /* DMP was off, and is turning on */
+    if (sensors & INV_DMP_PROCESSOR &&
+        !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
+        struct ext_slave_config config;
+        long odr;
+        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
+        config.len = sizeof(long);
+        config.apply = (state == INV_STATE_DMP_STARTED);
+        config.data = &odr;
+
+        odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000);
+        result = inv_mpu_config_accel(mldl_cfg,
+                                      inv_get_serial_handle(),
+                                      inv_get_serial_handle(), &config);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
+        odr = MPU_SLAVE_IRQ_TYPE_NONE;
+        result = inv_mpu_config_accel(mldl_cfg,
+                                      inv_get_serial_handle(),
+                                      inv_get_serial_handle(), &config);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        inv_init_fifo_hardare();
+    }
+
+    if (inv_obj.mode_change_func) {
+        result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    /* Get the fifo rate before changing sensors so if we need to match it */
+    fifoRate = inv_get_fifo_rate();
+    mldl_cfg->requested_sensors = sensors;
+
+    /* inv_dmp_start will turn the sensors on */
+    if (state == INV_STATE_DMP_STARTED) {
+        result = inv_dl_start(sensors);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_reset_motion();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_dl_stop(~sensors);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    inv_set_fifo_rate(fifoRate);
+
+    if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
+        struct ext_slave_config config;
+        long data;
+
+        config.len = sizeof(long);
+        config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
+        config.apply = (state == INV_STATE_DMP_STARTED);
+        config.data = &data;
+        data = MPU_SLAVE_IRQ_TYPE_DATA_READY;
+        result = inv_mpu_config_accel(mldl_cfg,
+                                      inv_get_serial_handle(),
+                                      inv_get_serial_handle(), &config);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    return result;
+}
+
+void inv_set_mode_change(inv_error_t(*mode_change_func)
+                         (unsigned long, unsigned long))
+{
+    inv_obj.mode_change_func = mode_change_func;
+}
+
+/**
+* Mantis setup
+*/
+#ifdef CONFIG_MPU_SENSORS_MPU6050B1
+inv_error_t inv_set_mpu_6050_config()
+{
+    long temp;
+    inv_error_t result;
+    unsigned char big8[4];
+    unsigned char atc[4];
+    long s[3], s2[3];
+    int kk;
+    struct mldl_cfg *mldlCfg = inv_get_dl_config();
+
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             0x0d, 4, atc);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    temp = atc[3] & 0x3f;
+    if (temp >= 32)
+        temp = temp - 64;
+    temp = (temp << 21) | 0x100000;
+    temp += (1L << 29);
+    temp = -temp;
+    result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    for (kk = 0; kk < 3; ++kk) {
+        s[kk] = atc[kk] & 0x3f;
+        if (s[kk] > 32)
+            s[kk] = s[kk] - 64;
+        s[kk] *= 2516582L;
+    }
+
+    for (kk = 0; kk < 3; ++kk) {
+        s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] +
+            mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] +
+            mldlCfg->pdata->orientation[kk * 3 + 2] * s[2];
+    }
+    result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+#endif
+
+/**
+ * @}
+ */
diff --git a/60xx/mlsdk/mllite/ml.h b/60xx/mlsdk/mllite/ml.h
new file mode 100644
index 0000000..838cd49
--- /dev/null
+++ b/60xx/mlsdk/mllite/ml.h
@@ -0,0 +1,596 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/******************************************************************************
+ *
+ * $Id: ml.h 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *****************************************************************************/
+
+/**
+ *  @defgroup ML
+ *  @brief  The Motion Library processes gyroscopes and accelerometers to
+ *          provide a physical model of the movement of the sensors.
+ *          The results of this processing may be used to control objects
+ *          within a user interface environment, detect gestures, track 3D
+ *          movement for gaming applications, and analyze the blur created
+ *          due to hand movement while taking a picture.
+ *
+ *  @{
+ *      @file ml.h
+ *      @brief Header file for the Motion Library.
+**/
+
+#ifndef INV_H
+#define INV_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+#include "mldmp.h"
+#include "mlsl.h"
+#include "mpu.h"
+#ifdef INV_INCLUDE_LEGACY_HEADERS
+#include "ml_legacy.h"
+#endif
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+
+/* - Module defines. - */
+
+/*************************************************************************/
+/*  Motion Library Vesion                                                */
+/*************************************************************************/
+
+#define INV_VERSION_MAJOR                 4
+#define INV_VERSION_MINOR                 0
+#define INV_VERSION_SUB_MINOR             0
+
+#define INV_VERSION_MAJOR_STR            "4"
+#define INV_VERSION_MINOR_STR            "0"
+#define INV_VERSION_SUB_MINOR_STR        "0"
+
+#define INV_VERSION_NONE                 ""
+#define INV_VERSION_PROTOTYPE            "ProtoA "
+#define INV_VERSION_ENGINEERING          "EngA "
+#define INV_VERSION_PRE_ALPHA            "Pre-Alpha "
+#define INV_VERSION_ALPHA                "Alpha "
+#define INV_VERSION_BETA                 "Beta "
+#define INV_VERSION_PRODUCTION           "Production "
+
+#ifndef INV_VERSION_TYPE
+#define INV_VERSION_TYPE INV_VERSION_NONE
+#endif
+
+#define INV_VERSION  "InvenSense MPL" " " \
+    "v" INV_VERSION_MAJOR_STR "." INV_VERSION_MINOR_STR "." INV_VERSION_SUB_MINOR_STR " " \
+    INV_VERSION_TYPE \
+    __DATE__ " " __TIME__
+
+/*************************************************************************/
+/*  Motion processing engines                                            */
+/*************************************************************************/
+#define INV_MOTION_DETECT                (0x0004)
+#define INV_BIAS_UPDATE                  (0x0008)
+#define INV_GESTURE                      (0x0020)
+#define INV_CONTROL                      (0x0040)
+#define INV_ORIENTATION                  (0x0100)
+#define INV_PEDOMETER                    (0x0200)
+#define INV_BASIC                        (INV_MOTION_DETECT | INV_BIAS_UPDATE)
+
+/*************************************************************************/
+/*  Data Source - Obsolete                                               */
+/*************************************************************************/
+#define INV_DATA_FIFO                    (0x1)
+#define INV_DATA_POLL                    (0x2)
+
+/*************************************************************************/
+/*  Interrupt Source                                                     */
+/*************************************************************************/
+#define INV_INT_MOTION                   (0x01)
+#define INV_INT_FIFO                     (0x02)
+#define INV_INT_TAP                      (0x04)
+#define INV_INT_ORIENTATION              (0x08)
+#define INV_INT_SHAKE_PITCH              (0x10)
+#define INV_INT_SHAKE_ROLL               (0x20)
+#define INV_INT_SHAKE_YAW                (0x40)
+
+/*************************************************************************/
+/*  Bias update functions                                                */
+/*************************************************************************/
+#define INV_BIAS_FROM_NO_MOTION          0x0001
+#define INV_BIAS_FROM_GRAVITY            0x0002
+#define INV_BIAS_FROM_TEMPERATURE        0x0004
+#define INV_BIAS_FROM_LPF                0x0008
+#define INV_MAG_BIAS_FROM_MOTION         0x0010
+#define INV_MAG_BIAS_FROM_GYRO           0x0020
+#define INV_LEARN_BIAS_FROM_TEMPERATURE  0x0040
+#define INV_AUTO_RESET_MAG_BIAS          0x0080
+#define INV_REJECT_MAG_DISTURBANCE       0x0100
+#define INV_PROGRESSIVE_NO_MOTION        0x0200
+#define INV_BIAS_FROM_FAST_NO_MOTION     0x0400
+
+/*************************************************************************/
+/*  Euler angles and axis names                                          */
+/*************************************************************************/
+#define INV_X_AXIS                       (0x01)
+#define INV_Y_AXIS                       (0x02)
+#define INV_Z_AXIS                       (0x04)
+
+#define INV_PITCH                        (INV_X_AXIS)
+#define INV_ROLL                         (INV_Y_AXIS)
+#define INV_YAW                          (INV_Z_AXIS)
+
+/*************************************************************************/
+/*  Sensor types                                                         */
+/*************************************************************************/
+#define INV_GYROS                        0x0001
+#define INV_ACCELS                       0x0002
+
+/*************************************************************************/
+/*  Motion arrays                                                        */
+/*************************************************************************/
+#define INV_ROTATION_MATRIX              0x0003
+#define INV_QUATERNION                   0x0004
+#define INV_EULER_ANGLES                 0x0005
+#define INV_LINEAR_ACCELERATION          0x0006
+#define INV_LINEAR_ACCELERATION_WORLD    0x0007
+#define INV_GRAVITY                      0x0008
+#define INV_ANGULAR_VELOCITY             0x0009
+
+#define INV_GYRO_CALIBRATION_MATRIX      0x000B
+#define INV_ACCEL_CALIBRATION_MATRIX     0x000C
+#define INV_GYRO_BIAS                    0x000D
+#define INV_ACCEL_BIAS                   0x000E
+#define INV_GYRO_TEMP_SLOPE              0x000F
+
+#define INV_RAW_DATA                     0x0011
+#define INV_DMP_TAP                      0x0012
+#define INV_DMP_TAP2                     0x0021
+
+#define INV_EULER_ANGLES_X               0x0013
+#define INV_EULER_ANGLES_Y               0x0014
+#define INV_EULER_ANGLES_Z               0x0015
+
+#define INV_BIAS_UNCERTAINTY             0x0016
+#define INV_DMP_PACKET_NUMBER            0x0017
+#define INV_FOOTER                       0x0018
+
+#define INV_CONTROL_DATA                 0x0019
+
+#define INV_MAGNETOMETER                 0x001A
+#define INV_PEDLBS                       0x001B
+#define INV_MAG_RAW_DATA                 0x001C
+#define INV_MAG_CALIBRATION_MATRIX       0x001D
+#define INV_MAG_BIAS                     0x001E
+#define INV_HEADING                      0x001F
+
+#define INV_MAG_BIAS_ERROR               0x0020
+
+#define INV_PRESSURE                     0x0021
+#define INV_LOCAL_FIELD                  0x0022
+#define INV_MAG_SCALE                    0x0023
+
+#define INV_RELATIVE_QUATERNION          0x0024
+
+#define SET_QUATERNION                                  0x0001
+#define SET_GYROS                                       0x0002
+#define SET_LINEAR_ACCELERATION                         0x0004
+#define SET_GRAVITY                                     0x0008
+#define SET_ACCELS                                      0x0010
+#define SET_TAP                                         0x0020
+#define SET_PEDLBS                                      0x0040
+#define SET_LINEAR_ACCELERATION_WORLD                   0x0080
+#define SET_CONTROL                                     0x0100
+#define SET_PACKET_NUMBER                               0x4000
+#define SET_FOOTER                                      0x8000
+
+/*************************************************************************/
+/*  Integral reset options                                               */
+/*************************************************************************/
+#define INV_NO_RESET                     0x0000
+#define INV_RESET                        0x0001
+
+/*************************************************************************/
+/*  Motion states                                                        */
+/*************************************************************************/
+#define INV_MOTION                       0x0001
+#define INV_NO_MOTION                    0x0002
+
+/*************************************************************************/
+/* Orientation and Gesture states                                        */
+/*************************************************************************/
+#define INV_STATE_IDLE                   (0)
+#define INV_STATE_RUNNING                (1)
+
+/*************************************************************************/
+/* Gyroscope Temperature Compensation bins                               */
+/*************************************************************************/
+#define BINS                            (25)
+#define PTS_PER_BIN                     (5)
+#define MIN_TEMP                        (-40)
+#define MAX_TEMP                        (+85)
+#define TEMP_PER_BIN                    ((MAX_TEMP - MIN_TEMP) / BINS)
+
+/*************************************************************************/
+/*  Flags                                                                */
+/*************************************************************************/
+#define INV_RAW_DATA_READY               0x0001
+#define INV_PROCESSED_DATA_READY         0x0002
+
+#define INV_GOT_GESTURE                  0x0004
+
+#define INV_MOTION_STATE_CHANGE          0x0006
+#define INV_COMPASS_OFFSET_VALID         0x0007
+
+/*************************************************************************/
+/*  General                                                              */
+/*************************************************************************/
+#define INV_NONE                         (0x0000)
+#define INV_INVALID_FIFO_RATE            (0xFFFF)
+
+/*************************************************************************/
+/*  ML Params Structure Default Values                                   */
+/*************************************************************************/
+#define INV_BIAS_UPDATE_FUNC_DEFAULT               (INV_BIAS_FROM_NO_MOTION | INV_BIAS_FROM_GRAVITY)
+#define INV_ORIENTATION_MASK_DEFAULT               0x3f
+#define INV_PROCESSED_DATA_CALLBACK_DEFAULT           0
+#define INV_ORIENTATION_CALLBACK_DEFAULT              0
+#define INV_MOTION_CALLBACK_DEFAULT                   0
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+/* Priority for various features */
+#define INV_PRIORITY_BUS_ACCEL              100
+#define INV_PRIORITY_EXTERNAL_SLAVE_MAG     110
+#define INV_PRIORITY_FAST_NO_MOTION         120
+#define INV_PRIORITY_BIAS_NO_MOTION         125
+#define INV_PRIORITY_SET_GYRO_BIASES        150
+#define INV_PRIORITY_TEMP_COMP              175
+#define INV_PRIORITY_CONTROL                200
+#define INV_PRIORITY_EIS                    300
+#define INV_PRIORITY_ORIENTATION            400
+#define INV_PRIORITY_PEDOMETER_FULLPOWER    500
+#define INV_PRIORITY_NAVIGATION_PEDOMETER   600
+#define INV_PRIORITY_GESTURE                700
+#define INV_PRIORITY_GLYPH                  800
+
+#define MAX_INTERRUPT_PROCESSES 5
+/* Number of quantized accel samples */
+#define INV_MAX_NUM_ACCEL_SAMPLES (8)
+
+#define PRECISION 10000.f
+#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) {               \
+    range.mantissa = (long)x;                               \
+    range.fraction = (long)((float)(x-(long)x)*PRECISION);  \
+}
+#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) {   \
+    x = (float)(range.mantissa);                \
+    x += ((float)range.fraction/PRECISION);     \
+}
+
+    /* --------------- */
+    /* - Structures. - */
+    /* --------------- */
+
+struct inv_obj_t {
+        //Calibration parameters
+        /* Raw sensor orientation */
+        long gyro_bias[3];
+        long accel_bias[3];
+        long compass_bias[3];
+
+        /* Cached values after orietnation is applied */
+        long scaled_gyro_bias[3];
+        long scaled_accel_bias[3];
+        long scaled_compass_bias[3];
+
+        long compass_scale[3];
+        long compass_test_bias[3];
+        long compass_test_scale[3];
+        long compass_asa[3];
+        long compass_offsets[3];
+
+        long compass_bias_error[3];
+
+        long got_no_motion_bias;
+        long got_compass_bias;
+        long compass_state;
+        long large_field;
+        long acc_state;
+
+        long factory_temp_comp;
+        long got_coarse_heading;
+        long gyro_temp_bias[3];
+        long prog_no_motion_bias[3];
+
+        long accel_cal[9];
+        // Deprecated, used gyro_orient
+        long gyro_cal[GYRO_NUM_AXES * GYRO_NUM_AXES];
+        long gyro_orient[GYRO_NUM_AXES * GYRO_NUM_AXES];
+        long accel_sens;
+        long compass_cal[9];
+        long gyro_sens;
+        long gyro_sf;
+        long temp_slope[GYRO_NUM_AXES];
+        long compass_sens;
+        long temp_offset[GYRO_NUM_AXES];
+
+        int cal_loaded_flag;
+
+        /* temperature compensation */
+        float x_gyro_coef[3];
+        float y_gyro_coef[3];
+        float z_gyro_coef[3];
+        float x_gyro_temp_data[BINS][PTS_PER_BIN];
+        float y_gyro_temp_data[BINS][PTS_PER_BIN];
+        float z_gyro_temp_data[BINS][PTS_PER_BIN];
+        float temp_data[BINS][PTS_PER_BIN];
+        int temp_ptrs[BINS];
+        long temp_valid_data[BINS];
+
+        long compass_correction[4];
+        long compass_correction_relative[4];
+        long compass_disturb_correction[4];
+        long compass_correction_offset[4];
+        long relative_quat[4];
+        long local_field[3];
+        long new_local_field;
+        long sync_grav_body[3];
+        int gyro_bias_err;
+
+        double compass_bias_ptr[9];
+        double compass_bias_v[3];
+        double compass_prev_m[36];
+        double compass_prev_xty[6];
+
+        int compass_peaks[18];
+        int all_sensors_no_motion;
+
+        long init_compass_bias[3];
+
+        int got_init_compass_bias;
+        int resetting_compass;
+
+        long ang_v_body[GYRO_NUM_AXES];
+        unsigned char compass_raw_data[24]; /* Sensor data plus status etc */
+        long compass_sensor_data[3]; /* Raw sensor data only */
+        long compass_calibrated_data[3];
+        long compass_test_calibrated_data[3];
+        long pressure;
+        inv_error_t (*external_slave_callback)(struct inv_obj_t *);
+        int  compass_accuracy;
+        int compass_overunder;
+
+        unsigned short flags[8];
+        unsigned short suspend;
+
+        long no_motion_threshold;
+        unsigned long motion_duration;
+
+        unsigned short motion_state;
+
+        unsigned short data_mode;
+        unsigned short interrupt_sources;
+
+        unsigned short bias_update_time;
+        short last_motion;
+        unsigned short bias_calc_time;
+
+        unsigned char internal_motion_state;
+        long start_time;
+
+        long accel_lpf_gain;
+        long accel_lpf[3];
+        unsigned long poll_no_motion;
+        long no_motion_accel_threshold;
+        unsigned long no_motion_accel_time;
+        inv_error_t(*mode_change_func) (unsigned long, unsigned long);
+    };
+
+    typedef inv_error_t(*inv_obj_func) (struct inv_obj_t *);
+
+    extern struct inv_obj_t inv_obj;
+
+    /* --------------------- */
+    /* - Params Structure. - */
+    /* --------------------- */
+
+    struct inv_params_obj {
+
+        unsigned short bias_mode;   // A function or bitwise OR of functions that determine how the gyroscope bias will be automatically updated.
+        // Functions include INV_BIAS_FROM_NO_MOTION, INV_BIAS_FROM_GRAVITY, and INV_BIAS_FROM_TEMPERATURE.
+        // The engine INV_BIAS_UPDATE must be enabled for these algorithms to run.
+
+        unsigned short orientation_mask;    // Allows a user to register which orientations will trigger the user defined callback function.
+        // The orientations are INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, and INV_Z_DOWN.
+        // INV_ORIENTATION_ALL is equivalent to INV_X_UP | INV_X_DOWN | INV_Y_UP | INV_Y_DOWN | INV_Z_UP | INV_Z_DOWN.
+
+        void (*fifo_processed_func) (void); // Callback function that triggers when all the processing has been finished by the motion processing engines.
+
+        void (*orientation_cb_func) (unsigned short orient);    // Callback function that will run when a change of orientation is detected.
+        // The new orientation. May be one of INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, or INV_Z_DOWN.
+
+        void (*motion_cb_func) (unsigned short motion_state);   // Callback function that will run when a change of motion state is detected.
+        // The new motion state. May be one of INV_MOTION, or INV_NO_MOTION.
+
+        unsigned char state;
+
+    };
+
+    extern struct inv_params_obj inv_params_obj;
+    /* --------------------- */
+    /* - Function p-types. - */
+    /* --------------------- */
+
+    inv_error_t inv_serial_start(char const *port);
+    inv_error_t inv_serial_stop(void);
+    inv_error_t inv_set_mpu_sensors(unsigned long sensors);
+    void *inv_get_serial_handle(void);
+
+    /*API for handling the buffer */
+    inv_error_t inv_update_data(void);
+
+    /*API for handling polling */
+    int inv_check_flag(int flag);
+
+    /*API for setting bias update function */
+    inv_error_t inv_set_bias_update(unsigned short biasFunction);
+
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+    inv_error_t inv_turn_on_bias_from_no_motion(void);
+    inv_error_t inv_turn_off_bias_from_no_motion(void);
+    inv_error_t inv_set_mpu_6050_config(void);
+#endif
+
+    /* Legacy functions for handling augmented data*/
+    inv_error_t inv_get_array(int dataSet, long *data);
+    inv_error_t inv_get_float_array(int dataSet, float *data);
+    inv_error_t inv_set_array(int dataSet, long *data);
+    inv_error_t inv_set_float_array(int dataSet, float *data);
+    /* Individual functions for augmented data, per specific dataset */
+
+
+    inv_error_t inv_get_gyro(long *data);
+    inv_error_t inv_get_accel(long *data);
+    inv_error_t inv_get_temperature(long *data);
+    inv_error_t inv_get_temperature_raw(short *data);
+    inv_error_t inv_get_rot_mat(long *data);
+    inv_error_t inv_get_quaternion(long *data);
+    inv_error_t inv_get_linear_accel(long *data);
+    inv_error_t inv_get_linear_accel_in_world(long *data);
+    inv_error_t inv_get_gravity(long *data);
+    inv_error_t inv_get_angular_velocity(long *data);
+    inv_error_t inv_get_euler_angles(long *data);
+    inv_error_t inv_get_euler_angles_x(long *data);
+    inv_error_t inv_get_euler_angles_y(long *data);
+    inv_error_t inv_get_euler_angles_z(long *data);
+    inv_error_t inv_get_gyro_temp_slope(long *data);
+    inv_error_t inv_get_gyro_bias(long *data);
+    inv_error_t inv_get_accel_bias(long *data);
+    inv_error_t inv_get_mag_bias(long *data);
+    inv_error_t inv_get_gyro_and_accel_sensor(long *data);
+    inv_error_t inv_get_mag_raw_data(long *data);
+    inv_error_t inv_get_magnetometer(long *data);
+    inv_error_t inv_get_pressure(long *data);
+    inv_error_t inv_get_heading(long *data);
+    inv_error_t inv_get_gyro_cal_matrix(long *data);
+    inv_error_t inv_get_accel_cal_matrix(long *data);
+    inv_error_t inv_get_mag_cal_matrix(long *data);
+    inv_error_t inv_get_mag_bias_error(long *data);
+    inv_error_t inv_get_mag_scale(long *data);
+    inv_error_t inv_get_local_field(long *data);
+    inv_error_t inv_get_relative_quaternion(long *data);
+    inv_error_t inv_get_gyro_float(float *data);
+    inv_error_t inv_get_accel_float(float *data);
+    inv_error_t inv_get_temperature_float(float *data);
+    inv_error_t inv_get_rot_mat_float(float *data);
+    inv_error_t inv_get_quaternion_float(float *data);
+    inv_error_t inv_get_linear_accel_float(float *data);
+    inv_error_t inv_get_linear_accel_in_world_float(float *data);
+    inv_error_t inv_get_gravity_float(float *data);
+    inv_error_t inv_get_angular_velocity_float(float *data);
+    inv_error_t inv_get_euler_angles_float(float *data);
+    inv_error_t inv_get_euler_angles_x_float(float *data);
+    inv_error_t inv_get_euler_angles_y_float(float *data);
+    inv_error_t inv_get_euler_angles_z_float(float *data);
+    inv_error_t inv_get_gyro_temp_slope_float(float *data);
+    inv_error_t inv_get_gyro_bias_float(float *data);
+    inv_error_t inv_get_accel_bias_float(float *data);
+    inv_error_t inv_get_mag_bias_float(float *data);
+    inv_error_t inv_get_gyro_and_accel_sensor_float(float *data);
+    inv_error_t inv_get_mag_raw_data_float(float *data);
+    inv_error_t inv_get_magnetometer_float(float *data);
+    inv_error_t inv_get_pressure_float(float *data);
+    inv_error_t inv_get_heading_float(float *data);
+    inv_error_t inv_get_gyro_cal_matrix_float(float *data);
+    inv_error_t inv_get_accel_cal_matrix_float(float *data);
+    inv_error_t inv_get_mag_cal_matrix_float(float *data);
+    inv_error_t inv_get_mag_bias_error_float(float *data);
+    inv_error_t inv_get_mag_scale_float(float *data);
+    inv_error_t inv_get_local_field_float(float *data);
+    inv_error_t inv_get_relative_quaternion_float(float *data);
+    inv_error_t inv_get_compass_accuracy(int *accuracy);
+    inv_error_t inv_set_gyro_bias(long *data);
+    inv_error_t inv_set_accel_bias(long *data);
+    inv_error_t inv_set_mag_bias(long *data);
+    inv_error_t inv_set_gyro_temp_slope(long *data);
+    inv_error_t inv_set_local_field(long *data);
+    inv_error_t inv_set_mag_scale(long *data);
+    inv_error_t inv_set_gyro_temp_slope_float(float *data);
+    inv_error_t inv_set_gyro_bias_float(float *data);
+    inv_error_t inv_set_accel_bias_float(float *data);
+    inv_error_t inv_set_mag_bias_float(float *data);
+    inv_error_t inv_set_local_field_float(float *data);
+    inv_error_t inv_set_mag_scale_float(float *data);
+
+    inv_error_t inv_apply_endian_accel(void);
+    inv_error_t inv_apply_calibration(void);
+    inv_error_t inv_set_gyro_calibration(float range, signed char *orientation);
+    inv_error_t inv_set_accel_calibration(float range,
+                                          signed char *orientation);
+    inv_error_t inv_set_compass_calibration(float range,
+                                            signed char *orientation);
+
+    /*API for detecting change of state */
+     inv_error_t
+        inv_set_motion_callback(void (*func) (unsigned short motion_state));
+    int inv_get_motion_state(void);
+
+    /*API for getting ML version. */
+    inv_error_t inv_get_version(unsigned char **version);
+
+    inv_error_t inv_set_motion_interrupt(unsigned char on);
+    inv_error_t inv_set_fifo_interrupt(unsigned char on);
+
+    int inv_get_interrupts(void);
+
+    /* Simulated DMP */
+    int inv_get_gyro_present(void);
+
+    inv_error_t inv_set_no_motion_time(float time);
+    inv_error_t inv_set_no_motion_thresh(float thresh);
+    inv_error_t inv_set_no_motion_threshAccel(long thresh);
+    inv_error_t inv_reset_motion(void);
+
+    inv_error_t inv_update_bias(void);
+    inv_error_t inv_set_dead_zone(void);
+    void inv_start_bias_calc(void);
+    void inv_stop_bias_calc(void);
+
+    // Private functions shared accross modules
+    void inv_init_ml(void);
+
+    inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func);
+    inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func);
+    void inv_run_dmp_interupt_cb(void);
+    void inv_set_mode_change(inv_error_t(*mode_change_func)
+                              (unsigned long, unsigned long));
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          // INV_H
+/**
+ * @}
+ */
diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.c b/60xx/mlsdk/mllite/mlBiasNoMotion.c
new file mode 100644
index 0000000..73321ff
--- /dev/null
+++ b/60xx/mlsdk/mllite/mlBiasNoMotion.c
@@ -0,0 +1,393 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+#include "mlBiasNoMotion.h"
+#include "ml.h"
+#include "mlinclude.h"
+#include "mlos.h"
+#include "mlFIFO.h"
+#include "dmpKey.h"
+#include "accel.h"
+#include "mlMathFunc.h"
+#include "mldl.h"
+#include "mlstates.h"
+#include "mlSetGyroBias.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-BiasNoMot"
+
+
+#define _mlDebug(x)             //{x}
+
+/**
+ *  @brief  inv_set_motion_callback is used to register a callback function that
+ *          will trigger when a change of motion state is detected.
+ *
+ *  @pre    inv_dmp_open() 
+ *          @ifnot MPL_MF 
+ *              or inv_open_low_power_pedometer() 
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start()
+ *          must <b>NOT</b> have been called.
+ *
+ *  @param  func    A user defined callback function accepting a
+ *                  motion_state parameter, the new motion state.
+ *                  May be one of INV_MOTION or INV_NO_MOTION.
+ *  @return INV_SUCCESS if successful or Non-zero error code otherwise.
+ */
+inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state))
+{
+    INVENSENSE_FUNC_START;
+
+    if ((inv_get_state() != INV_STATE_DMP_OPENED) &&
+        (inv_get_state() != INV_STATE_DMP_STARTED))
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    inv_params_obj.motion_cb_func = func;
+
+    return INV_SUCCESS;
+}
+
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+/** Turns on the feature to compute gyro bias from No Motion */
+inv_error_t inv_turn_on_bias_from_no_motion()
+{
+    inv_error_t result;
+    unsigned char regs[3] = { 0x0d, DINA35, 0x5d };
+    inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
+    result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
+    return result;
+}
+
+/** Turns off the feature to compute gyro bias from No Motion
+*/
+inv_error_t inv_turn_off_bias_from_no_motion()
+{
+    inv_error_t result;
+    unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 };
+    inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
+    result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
+    return result;
+}
+#endif
+
+inv_error_t inv_update_bias(void)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    unsigned char regs[12];
+    short bias[GYRO_NUM_AXES];
+
+    if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION)
+        && inv_get_gyro_present()) {
+
+        regs[0] = DINAA0 + 3;
+        result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_get_mpu_memory(KEY_D_1_244, 12, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        inv_convert_bias(regs, bias);
+
+        regs[0] = DINAA0 + 15;
+        result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        result =
+            inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                            MPUREG_TEMP_OUT_H, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        inv_obj.got_no_motion_bias = TRUE;
+    }
+    return INV_SUCCESS;
+}
+
+inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj)
+{
+    long gain;
+    unsigned long timeChange;
+    long rate;
+    inv_error_t result;
+    long accel[3], temp;
+    long long accelMag;
+    unsigned long currentTime;
+    int kk;
+
+    if (!inv_accel_present()) {
+        return INV_SUCCESS;
+    }
+
+    currentTime = inv_get_tick_count();
+
+    // We always run the accel low pass filter at the highest sample rate possible
+    result = inv_get_accel(accel);
+    if (result != INV_ERROR_FEATURE_NOT_ENABLED) {
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        rate = inv_get_fifo_rate() * 5 + 5;
+        if (rate > 200)
+            rate = 200;
+
+        gain = inv_obj->accel_lpf_gain * rate;
+        timeChange = inv_get_fifo_rate();
+
+        accelMag = 0;
+        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+            inv_obj->accel_lpf[kk] =
+                inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]);
+            inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]);
+            temp = accel[0] - inv_obj->accel_lpf[0];
+            accelMag += (long long)temp *temp;
+        }
+
+        if (accelMag > inv_obj->no_motion_accel_threshold) {
+            inv_obj->no_motion_accel_time = currentTime;
+
+            // Check for change of state
+            if (!inv_get_gyro_present())
+                inv_set_motion_state(INV_MOTION);
+
+        } else if ((currentTime - inv_obj->no_motion_accel_time) >
+                   5 * inv_obj->motion_duration) {
+            // We have no motion according to accel
+            // Check fsor change of state
+            if (!inv_get_gyro_present())
+                inv_set_motion_state(INV_NO_MOTION);
+        }
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * @brief   Manually update the motion/no motion status.  This is a 
+ *          convienence function for implementations that do not wish to use 
+ *          inv_update_data.  
+ *          This function can be called periodically to check for the 
+ *          'no motion' state and update the internal motion status and bias 
+ *          calculations.
+ */
+inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[3] = { 0 };
+    unsigned short motionFlag = 0;
+    unsigned long currentTime;
+    inv_error_t result;
+
+    result = MLAccelMotionDetection(inv_obj);
+
+    currentTime = inv_get_tick_count();
+
+    // If it is not time to poll for a no motion event, return
+    if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) &&
+        ((currentTime - inv_obj->poll_no_motion) <= 1000))
+        return INV_SUCCESS;
+
+    inv_obj->poll_no_motion = currentTime;
+
+#if defined CONFIG_MPU_SENSORS_MPU3050
+    if (inv_get_gyro_present()
+        && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
+        static long repeatBiasUpdateTime = 0;
+
+        result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1];
+
+        _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);
+            )
+            if (motionFlag == inv_obj->motion_duration) {
+            if (inv_obj->motion_state == INV_MOTION) {
+                inv_update_bias();
+                repeatBiasUpdateTime = inv_get_tick_count();
+
+                regs[0] = DINAD8 + 1;
+                regs[1] = DINA0C;
+                regs[2] = DINAD8 + 2;
+                result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                    return result;
+                }
+
+                regs[0] = 0;
+                regs[1] = 5;
+                result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                    return result;
+                }
+
+                //Trigger no motion callback
+                inv_set_motion_state(INV_NO_MOTION);
+            }
+        }
+        if (motionFlag == 5) {
+            if (inv_obj->motion_state == INV_NO_MOTION) {
+                regs[0] = DINAD8 + 2;
+                regs[1] = DINA0C;
+                regs[2] = DINAD8 + 1;
+                result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                    return result;
+                }
+
+                regs[0] =
+                    (unsigned char)((inv_obj->motion_duration >> 8) & 0xff);
+                regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff);
+                result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                    return result;
+                }
+
+                //Trigger no motion callback
+                inv_set_motion_state(INV_MOTION);
+            }
+        }
+        if (inv_obj->motion_state == INV_NO_MOTION) {
+            if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) {
+                inv_update_bias();
+                repeatBiasUpdateTime = inv_get_tick_count();
+            }
+        }
+    }
+#else                           // CONFIG_MPU_SENSORS_MPU3050
+    if (inv_get_gyro_present()
+        && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
+        result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1];
+
+        _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);
+            )
+            if (motionFlag > 0) {
+            unsigned char biasReg[12];
+            long biasTmp2[3], biasTmp[3];
+            int i;
+
+            if (inv_obj->last_motion != motionFlag) {
+                result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg);
+
+                for (i = 0; i < 3; i++) {
+                    biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]);
+                }
+                // Rotate bias vector by the transpose of the orientation matrix
+                for (i = 0; i < 3; ++i) {
+                    biasTmp[i] =
+                        inv_q30_mult(biasTmp2[0],
+                                     inv_obj->gyro_orient[i]) +
+                        inv_q30_mult(biasTmp2[1],
+                                     inv_obj->gyro_orient[i + 3]) +
+                        inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]);
+                }
+                inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L);
+                inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L);
+                inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L);
+            }
+            inv_set_motion_state(INV_NO_MOTION);
+        } else {
+            // We are in a motion state
+            inv_set_motion_state(INV_MOTION);
+        }
+        inv_obj->last_motion = motionFlag;
+
+    }
+#endif                          // CONFIG_MPU_SENSORS_MPU3050
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_enable_bias_no_motion(void)
+{
+    inv_error_t result;
+    inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
+    result =
+        inv_register_fifo_rate_process(MLPollMotionStatus,
+                                       INV_PRIORITY_BIAS_NO_MOTION);
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_turn_on_bias_from_no_motion();
+#endif
+    return result;
+}
+
+inv_error_t inv_disable_bias_no_motion(void)
+{
+    inv_error_t result;
+    inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
+    result = inv_unregister_fifo_rate_process(MLPollMotionStatus);
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+    defined CONFIG_MPU_SENSORS_MPU6050B1
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_turn_off_bias_from_no_motion();
+#endif
+    return result;
+}
diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.h b/60xx/mlsdk/mllite/mlBiasNoMotion.h
new file mode 100644
index 0000000..030dbf9
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlFIFO.c b/60xx/mlsdk/mllite/mlFIFO.c
new file mode 100644
index 0000000..2c0d2f0
--- /dev/null
+++ b/60xx/mlsdk/mllite/mlFIFO.c
@@ -0,0 +1,2265 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *******************************************************************************/
+
+/**
+ *   @defgroup MLFIFO
+ *   @brief Motion Library - FIFO Driver.
+ *          The FIFO API Interface.
+ *
+ *   @{
+ *       @file mlFIFO.c
+ *       @brief FIFO Interface.
+**/
+
+#include <string.h>
+#include "mpu.h"
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+#    include "mpu6050a2.h"
+#elif defined CONFIG_MPU_SENSORS_MPU6050B1
+#    include "mpu6050b1.h"
+#elif defined CONFIG_MPU_SENSORS_MPU3050
+#  include "mpu3050.h"
+#else
+#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
+#endif
+#include "mlFIFO.h"
+#include "mlFIFOHW.h"
+#include "dmpKey.h"
+#include "mlMathFunc.h"
+#include "ml.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "mlstates.h"
+#include "mlsupervisor.h"
+#include "mlos.h"
+#include "mlmath.h"
+#include "accel.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-fifo"
+
+#define FIFO_DEBUG 0
+
+#define REF_QUATERNION             (0)
+#define REF_GYROS                  (REF_QUATERNION + 4)
+#define REF_CONTROL                (REF_GYROS + 3)
+#define REF_RAW                    (REF_CONTROL + 4)
+#define REF_RAW_EXTERNAL           (REF_RAW + 8)
+#define REF_ACCEL                  (REF_RAW_EXTERNAL + 6)
+#define REF_QUANT_ACCEL            (REF_ACCEL + 3)
+#define REF_QUATERNION_6AXIS       (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES)
+#define REF_EIS                    (REF_QUATERNION_6AXIS + 4)
+#define REF_DMP_PACKET             (REF_EIS + 3)
+#define REF_GARBAGE                (REF_DMP_PACKET + 1)
+#define REF_LAST                   (REF_GARBAGE + 1)
+
+long fifo_scale[REF_LAST] = {
+    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion
+    // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2)
+    1501974482L, 1501974482L, 1501974482L,  // Gyro
+    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control
+    (1L << 14),                 // Temperature
+    (1L << 14), (1L << 14), (1L << 14), // Raw Gyro
+    (1L << 14), (1L << 14), (1L << 14), (0),    // Raw Accel, plus padding
+    (1L << 14), (1L << 14), (1L << 14), // Raw External
+    (1L << 14), (1L << 14), (1L << 14), // Raw External
+    (1L << 16), (1L << 16), (1L << 16), // Accel
+    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel
+    (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel
+    (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis
+    (1L << 30), (1L << 30), (1L << 30), // EIS
+    (1L << 30),                 // Packet
+    (1L << 30),                 // Garbage
+};
+
+// The scale factors for tap need to match the number in fifo_scale above.
+// fifo_base_offset below may also need to be changed if this is not 8
+#if INV_MAX_NUM_ACCEL_SAMPLES != 8
+#error  INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8
+#endif
+
+#define CONFIG_QUAT                (0)
+#define CONFIG_GYROS               (CONFIG_QUAT + 1)
+#define CONFIG_CONTROL_DATA        (CONFIG_GYROS + 1)
+#define CONFIG_TEMPERATURE         (CONFIG_CONTROL_DATA + 1)
+#define CONFIG_RAW_DATA            (CONFIG_TEMPERATURE + 1)
+#define CONFIG_RAW_EXTERNAL        (CONFIG_RAW_DATA + 1)
+#define CONFIG_ACCEL               (CONFIG_RAW_EXTERNAL + 1)
+#define CONFIG_DMP_QUANT_ACCEL     (CONFIG_ACCEL + 1)
+#define CONFIG_EIS                 (CONFIG_DMP_QUANT_ACCEL + 1)
+#define CONFIG_DMP_PACKET_NUMBER   (CONFIG_EIS + 1)
+#define CONFIG_FOOTER              (CONFIG_DMP_PACKET_NUMBER + 1)
+#define NUMFIFOELEMENTS            (CONFIG_FOOTER + 1)
+
+const int fifo_base_offset[NUMFIFOELEMENTS] = {
+    REF_QUATERNION * 4,
+    REF_GYROS * 4,
+    REF_CONTROL * 4,
+    REF_RAW * 4,
+    REF_RAW * 4 + 4,
+    REF_RAW_EXTERNAL * 4,
+    REF_ACCEL * 4,
+    REF_QUANT_ACCEL * 4,
+    REF_EIS * 4,
+    REF_DMP_PACKET * 4,
+    REF_GARBAGE * 4
+};
+
+struct fifo_obj {
+    void (*fifo_process_cb) (void);
+    long decoded[REF_LAST];
+    long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES];
+    int offsets[REF_LAST * 4];
+    int cache;
+    uint_fast8_t gyro_source;
+    unsigned short fifo_rate;
+    unsigned short sample_step_size_ms;
+    uint_fast16_t fifo_packet_size;
+    uint_fast16_t data_config[NUMFIFOELEMENTS];
+    unsigned char reference_count[REF_LAST];
+    long acc_bias_filt[3];
+    float acc_filter_coef;
+    long gravity_cache[3];
+};
+
+static struct fifo_obj fifo_obj;
+
+#define FIFO_CACHE_TEMPERATURE 1
+#define FIFO_CACHE_GYRO 2
+#define FIFO_CACHE_GRAVITY_BODY 4
+#define FIFO_CACHE_ACC_BIAS 8
+
+struct fifo_rate_obj {
+    // These describe callbacks happening everytime a FIFO block is processed
+    int_fast8_t num_cb;
+    HANDLE mutex;
+    inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES];
+    int priority[MAX_HIGH_RATE_PROCESSES];
+};
+
+struct fifo_rate_obj fifo_rate_obj;
+
+/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old 
+ *  accuracy if needed.
+ */
+static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements,
+                                           uint_fast16_t accuracy,
+                                           uint_fast8_t configOffset)
+{
+    if (elements) {
+        if (!accuracy)
+            accuracy = fifo_obj.data_config[configOffset];
+        else if (accuracy & INV_16_BIT)
+            if ((fifo_obj.data_config[configOffset] & INV_32_BIT))
+                accuracy = INV_32_BIT;  // 32-bits takes priority
+            else
+                accuracy = INV_16_BIT;
+        else
+            accuracy = INV_32_BIT;
+    } else {
+        accuracy = 0;
+    }
+
+    return accuracy;
+}
+
+/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, 
+ * the reference counts are subtracted, otherwise they are incremented for each
+ * bit set in element. The value returned are the elements that should be sent
+ * out as data through the FIFO.
+*/
+static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements,
+                                            uint_fast16_t increment,
+                                            uint_fast8_t refOffset,
+                                            uint_fast8_t len)
+{
+    uint_fast8_t kk;
+
+    if (increment == 0) {
+        for (kk = 0; kk < len; ++kk) {
+            if ((elements & 1)
+                && (fifo_obj.reference_count[kk + refOffset] > 0)) {
+                fifo_obj.reference_count[kk + refOffset]--;
+            }
+            elements >>= 1;
+        }
+    } else {
+        for (kk = 0; kk < len; ++kk) {
+            if (elements & 1)
+                fifo_obj.reference_count[kk + refOffset]++;
+            elements >>= 1;
+        }
+    }
+    elements = 0;
+    for (kk = 0; kk < len; ++kk) {
+        if (fifo_obj.reference_count[kk + refOffset] > 0)
+            elements |= (1 << kk);
+    }
+    return elements;
+}
+
+/**
+ * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send
+ *  out the FIFO, 0 when removing from the FIFO.
+ */
+static inv_error_t inv_construct3_fifo(unsigned char *regs,
+                                       uint_fast16_t elements,
+                                       uint_fast16_t accuracy,
+                                       uint_fast8_t refOffset,
+                                       unsigned short key,
+                                       uint_fast8_t configOffset)
+{
+    int_fast8_t kk;
+    inv_error_t result;
+
+    elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3);
+    accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset);
+
+    if (accuracy & INV_16_BIT) {
+        regs[0] = DINAF8 + 2;
+    }
+
+    fifo_obj.data_config[configOffset] = elements | accuracy;
+
+    for (kk = 0; kk < 3; ++kk) {
+        if ((elements & 1) == 0)
+            regs[kk + 1] = DINAA0 + 3;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(key, 4, regs);
+
+    return result;
+}
+
+/** 
+ * @internal
+ * Puts footer on FIFO data.
+ */
+static inv_error_t inv_set_footer(void)
+{
+    unsigned char regs = DINA30;
+    uint_fast8_t tmp_count;
+    int_fast8_t i, j;
+    int offset;
+    int result;
+    int *fifo_offsets_ptr = fifo_obj.offsets;
+
+    fifo_obj.fifo_packet_size = 0;
+    for (i = 0; i < NUMFIFOELEMENTS; i++) {
+        tmp_count = 0;
+        offset = fifo_base_offset[i];
+        for (j = 0; j < 8; j++) {
+            if ((fifo_obj.data_config[i] >> j) & 0x0001) {
+#ifndef BIG_ENDIAN
+                // Special Case for Byte Ordering on Accel Data
+                if ((i == CONFIG_RAW_DATA) && (j > 2)) {
+                    tmp_count += 2;
+                    switch (inv_get_dl_config()->accel->endian) {
+                    case EXT_SLAVE_BIG_ENDIAN:
+                        *fifo_offsets_ptr++ = offset + 3;
+                        *fifo_offsets_ptr++ = offset + 2;
+                        break;
+                    case EXT_SLAVE_LITTLE_ENDIAN:
+                        *fifo_offsets_ptr++ = offset + 2;
+                        *fifo_offsets_ptr++ = offset + 3;
+                        break;
+                    case EXT_SLAVE_FS8_BIG_ENDIAN:
+                        if (j == 3) {
+                            // Throw this byte away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ = offset + 3;
+                        } else if (j == 4) {
+                            *fifo_offsets_ptr++ = offset + 3;
+                            *fifo_offsets_ptr++ = offset + 7;
+                        } else {
+                            // Throw these byte away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                        }
+                        break;
+                    case EXT_SLAVE_FS16_BIG_ENDIAN:
+                        if (j == 3) {
+                            // Throw this byte away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ = offset + 3;
+                        } else if (j == 4) {
+                            *fifo_offsets_ptr++ = offset - 2;
+                            *fifo_offsets_ptr++ = offset + 3;
+                        } else {
+                            *fifo_offsets_ptr++ = offset - 2;
+                            *fifo_offsets_ptr++ = offset + 3;
+                        }
+                        break;
+                    default:
+                        return INV_ERROR;    // Bad value on ordering
+                    }
+                } else {
+                    tmp_count += 2;
+                    *fifo_offsets_ptr++ = offset + 3;
+                    *fifo_offsets_ptr++ = offset + 2;
+                    if (fifo_obj.data_config[i] & INV_32_BIT) {
+                        *fifo_offsets_ptr++ = offset + 1;
+                        *fifo_offsets_ptr++ = offset;
+                        tmp_count += 2;
+                    }
+                }
+#else
+                // Big Endian Platform
+                // Special Case for Byte Ordering on Accel Data
+                if ((i == CONFIG_RAW_DATA) && (j > 2)) {
+                    tmp_count += 2;
+                    switch (inv_get_dl_config()->accel->endian) {
+                    case EXT_SLAVE_BIG_ENDIAN:
+                        *fifo_offsets_ptr++ = offset + 2;
+                        *fifo_offsets_ptr++ = offset + 3;
+                        break;
+                    case EXT_SLAVE_LITTLE_ENDIAN:
+                        *fifo_offsets_ptr++ = offset + 3;
+                        *fifo_offsets_ptr++ = offset + 2;
+                        break;
+                    case EXT_SLAVE_FS8_BIG_ENDIAN:
+                        if (j == 3) {
+                            // Throw this byte away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ = offset;
+                        } else if (j == 4) {
+                            *fifo_offsets_ptr++ = offset;
+                            *fifo_offsets_ptr++ = offset + 4;
+                        } else {
+                            // Throw these bytes away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                        }
+                        break;
+                    case EXT_SLAVE_FS16_BIG_ENDIAN:
+                        if (j == 3) {
+                            // Throw this byte away
+                            *fifo_offsets_ptr++ =
+                                fifo_base_offset[CONFIG_FOOTER];
+                            *fifo_offsets_ptr++ = offset;
+                        } else if (j == 4) {
+                            *fifo_offsets_ptr++ = offset - 3;
+                            *fifo_offsets_ptr++ = offset;
+                        } else {
+                            *fifo_offsets_ptr++ = offset - 3;
+                            *fifo_offsets_ptr++ = offset;
+                        }
+                        break;
+                    default:
+                        return INV_ERROR;    // Bad value on ordering
+                    }
+                } else {
+                    tmp_count += 2;
+                    *fifo_offsets_ptr++ = offset;
+                    *fifo_offsets_ptr++ = offset + 1;
+                    if (fifo_obj.data_config[i] & INV_32_BIT) {
+                        *fifo_offsets_ptr++ = offset + 2;
+                        *fifo_offsets_ptr++ = offset + 3;
+                        tmp_count += 2;
+                    }
+                }
+
+#endif
+            }
+            offset += 4;
+        }
+        fifo_obj.fifo_packet_size += tmp_count;
+    }
+    if (fifo_obj.data_config[CONFIG_FOOTER] == 0 &&
+        fifo_obj.fifo_packet_size > 0) {
+        // Add footer
+        result = inv_set_mpu_memory(KEY_CFG_16, 1, &regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT;
+        fifo_obj.fifo_packet_size += 2;
+    } else if (fifo_obj.data_config[CONFIG_FOOTER] &&
+               (fifo_obj.fifo_packet_size == 2)) {
+        // Remove Footer
+        regs = DINAA0 + 3;
+        result = inv_set_mpu_memory(KEY_CFG_16, 1, &regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        fifo_obj.data_config[CONFIG_FOOTER] = 0;
+        fifo_obj.fifo_packet_size = 0;
+    }
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_decode_quantized_accel(void)
+{
+    int kk;
+    int fifoRate = inv_get_fifo_rate();
+
+    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1));
+         kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) {
+        union {
+            unsigned int u10:10;
+            signed int s10:10;
+        } temp;
+
+        union {
+            uint32_t u32;
+            int32_t s32;
+        } value;
+
+        value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk];
+        // unquantize this samples.  
+        // They are stored as x * 2^20 + y * 2^10 + z
+        // Z
+        temp.u10 = value.u32 & 0x3ff;
+        value.s32 -= temp.s10;
+        fifo_obj.decoded_accel[kk][2] = temp.s10 * 64;
+        // Y
+        value.s32 = value.s32 / 1024;
+        temp.u10 = value.u32 & 0x3ff;
+        value.s32 -= temp.s10;
+        fifo_obj.decoded_accel[kk][1] = temp.s10 * 64;
+        // X
+        value.s32 = value.s32 / 1024;
+        temp.u10 = value.u32 & 0x3ff;
+        fifo_obj.decoded_accel[kk][0] = temp.s10 * 64;
+    }
+    return INV_SUCCESS;
+}
+
+static inv_error_t inv_state_change_fifo(unsigned char newState)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned char regs[4];
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    /* Don't reset the fifo on a fifo rate change */
+    if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
+        (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) {
+        /* Delay output on restart by 50ms due to warm up time of gyros */
+
+        short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1);
+        inv_init_fifo_hardare();
+        inv_int16_to_big8(delay, regs);
+        result = inv_set_mpu_memory(KEY_D_1_178, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    if (INV_STATE_DMP_STARTED == newState) {
+        if (inv_dmpkey_supported(KEY_D_1_128)) {
+            double tmp;
+            tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1);
+            if (tmp > 0x40000000L)
+                tmp = 0x40000000L;
+            (void)inv_int32_to_big8((long)tmp, regs);
+            result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            result = inv_reset_fifo();
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+    }
+    return result;
+}
+
+/**
+ * @internal
+ * @brief get the FIFO packet size
+ * @return the FIFO packet size
+ */
+uint_fast16_t inv_get_fifo_packet_size(void)
+{
+    return fifo_obj.fifo_packet_size;
+}
+
+/**
+ *  @brief  Initializes all the internal static variables for 
+ *          the FIFO module.
+ *  @note   Should be called by the initialization routine such 
+ *          as inv_dmp_open().
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise. 
+ */
+inv_error_t inv_init_fifo_param(void)
+{
+    inv_error_t result;
+    memset(&fifo_obj, 0, sizeof(struct fifo_obj));
+    fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity
+    inv_set_linear_accel_filter_coef(0.f);
+    fifo_obj.fifo_rate = 20;
+    fifo_obj.sample_step_size_ms = 100;
+    memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
+    result = inv_create_mutex(&fifo_rate_obj.mutex);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_register_state_callback(inv_state_change_fifo);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  Close the FIFO usage.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_close_fifo(void)
+{
+    inv_error_t result;
+    inv_error_t first = INV_SUCCESS;
+    result = inv_unregister_state_callback(inv_state_change_fifo);
+    ERROR_CHECK_FIRST(first, result);
+    result = inv_destroy_mutex(fifo_rate_obj.mutex);
+    ERROR_CHECK_FIRST(first, result);
+    memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
+    return first;
+}
+
+/** 
+ * Set the gyro source to output to the fifo
+ * 
+ * @param source The source.  One of 
+ * - INV_GYRO_FROM_RAW
+ * - INV_GYRO_FROM_QUATERNION
+ * 
+ * @return INV_SUCCESS or non-zero error code;
+ */
+inv_error_t inv_set_gyro_data_source(uint_fast8_t source)
+{
+    if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    fifo_obj.gyro_source = source;
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  Reads and processes FIFO data. Also handles callbacks when data is
+ *          ready.
+ *  @param  numPackets 
+ *              Number of FIFO packets to try to read. You should
+ *              use a large number here, such as 100, if you want to read all
+ *              the full packets in the FIFO, which is typical operation.
+ *  @param  processed
+ *              The number of FIFO packets processed. This may be incremented
+ *              even if high rate processes later fail.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
+                                      int_fast8_t * processed)
+{
+    int_fast8_t packet;
+    inv_error_t result = INV_SUCCESS;
+    uint_fast16_t read;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+    int kk;
+
+    if (NULL == processed)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    *processed = 0;
+    if (fifo_obj.fifo_packet_size == 0)
+        return result;          // Nothing to read
+
+    for (packet = 0; packet < numPackets; ++packet) {
+        if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
+            unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE];
+            unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE];
+            read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size,
+                                footer_n_data);
+            if (0 == read ||
+                read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) {
+                result = inv_get_fifo_status();
+                if (INV_SUCCESS != result) {
+                    memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
+                }
+                return result;
+            }
+
+            result = inv_process_fifo_packet(buf);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        } else if (inv_accel_present()) {
+            long data[ACCEL_NUM_AXES];
+            result = inv_get_accel_data(data);
+            if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
+                return INV_SUCCESS;
+            }
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+
+            memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
+            fifo_obj.cache = 0;
+            for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+                fifo_obj.decoded[REF_RAW + 4 + kk] =
+                    inv_q30_mult((data[kk] << 16),
+                                 fifo_scale[REF_RAW + 4 + kk]);
+                fifo_obj.decoded[REF_ACCEL + kk] =
+                    inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]);
+                fifo_obj.decoded[REF_ACCEL + kk] -=
+                    inv_obj.scaled_accel_bias[kk];
+            }
+        }
+        // The buffer was processed correctly, so increment even if
+        // other processes fail later, which will return an error
+        *processed = *processed + 1;
+
+        if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) &&
+            fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) {
+            result = inv_decode_quantized_accel();
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+
+        if (fifo_obj.data_config[CONFIG_QUAT]) {
+            result = inv_accel_compass_supervisor();
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+
+        result = inv_pressure_supervisor();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        // Callbacks now that we have a buffer of data ready
+        result = inv_run_fifo_rate_processes();
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+    }
+    return result;
+}
+
+/**
+ *  @brief  inv_set_fifo_processed_callback is used to set a processed data callback
+ *          function.  inv_set_fifo_processed_callback sets a user defined callback
+ *          function that triggers when all the decoding has been finished by
+ *          the motion processing engines. It is called before other bigger 
+ *          processing engines to allow lower latency for the user.
+ *
+ *  @pre    inv_dmp_open() 
+ *          @ifnot MPL_MF 
+ *              or inv_open_low_power_pedometer() 
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start() 
+ *          must <b>NOT</b> have been called.
+ *
+ *  @param  func    A user defined callback function.
+ *
+ *  @return INV_SUCCESS if successful, or non-zero error code otherwise.
+ */
+inv_error_t inv_set_fifo_processed_callback(void (*func) (void))
+{
+    INVENSENSE_FUNC_START;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    fifo_obj.fifo_process_cb = func;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * @brief   Process data from the dmp read via the fifo.  Takes a buffer 
+ *          filled with bytes read from the DMP FIFO. 
+ *          Currently expects only the 16 bytes of quaternion data. 
+ *          Calculates the motion parameters from that data and stores the 
+ *          results in an internal data structure.
+ * @param[in,out]   dmpData     Pointer to the buffer containing the fifo data.
+ * @return  INV_SUCCESS or error code.
+**/
+inv_error_t inv_process_fifo_packet(const unsigned char *dmpData)
+{
+    INVENSENSE_FUNC_START;
+    int N, kk;
+    unsigned char *p;
+
+    p = (unsigned char *)(&fifo_obj.decoded);
+    N = fifo_obj.fifo_packet_size;
+    if (N > sizeof(fifo_obj.decoded))
+        return INV_ERROR_ASSERTION_FAILURE;
+
+    memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
+
+    for (kk = 0; kk < N; ++kk) {
+        p[fifo_obj.offsets[kk]] = *dmpData++;
+    }
+
+    // If multiplies are much greater cost than if checks, you could check
+    // to see if fifo_scale is non-zero first, or equal to (1L<<30)
+    for (kk = 0; kk < REF_LAST; ++kk) {
+        fifo_obj.decoded[kk] =
+            inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]);
+    }
+
+    memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS],
+           &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long));
+
+    inv_obj.flags[INV_PROCESSED_DATA_READY] = 1;
+    fifo_obj.cache = 0;
+
+    return INV_SUCCESS;
+}
+
+/** Converts 16-bit temperature data as read from temperature register
+* into Celcius scaled by 2^16.
+*/
+long inv_decode_temperature(short tempReg)
+{
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+    // Celcius = 35 + (T + 3048.7)/325.9
+    return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L);
+#endif
+#if defined CONFIG_MPU_SENSORS_MPU6050B1
+    // Celcius = 35 + (T + 927.4)/360.6
+    return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L);
+#endif
+#if defined CONFIG_MPU_SENSORS_MPU3050
+    // Celcius = 35 + (T + 13200)/280
+    return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L);
+#endif
+}
+
+/**  @internal
+* Returns the temperature in hardware units. The scaling may change from part to part.
+*/
+inv_error_t inv_get_temperature_raw(short *data)
+{
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) {
+        inv_error_t result;
+        unsigned char regs[2];
+        if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) {
+            if (FIFO_DEBUG)
+                MPL_LOGI("Fetching the temperature from the registers\n");
+            fifo_obj.cache |= FIFO_CACHE_TEMPERATURE;
+            result = inv_serial_read(inv_get_serial_handle(),
+                                inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2,
+                                regs);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]);
+        }
+    }
+    *data = (short)fifo_obj.decoded[REF_RAW];
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 1-element vector of temperature. It is read from the hardware if it
+ *              doesn't exist in the FIFO.
+ *  @param[out] data    1-element vector of temperature
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_temperature(long *data)
+{
+    short tr;
+    inv_error_t result;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+    result = inv_get_temperature_raw(&tr);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    data[0] = inv_decode_temperature(tr);
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Get the Decoded Accel Data.
+ *  @param  data
+ *              a buffer to store the quantized data.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_get_unquantized_accel(long *data)
+{
+    int ii, kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
+        for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
+            data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk];
+        }
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Get the Quantized Accel data algorithm output from the FIFO.
+ *  @param  data
+ *              a buffer to store the quantized data.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_get_quantized_accel(long *data)
+{
+    int ii;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
+        data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii];
+    }
+
+    return INV_SUCCESS;
+}
+
+/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO
+*  and it is read from the registers if it was not put into the FIFO. The data is
+*  cached till the next FIFO processing block time.
+* @param[out] data Length 3, Gyro data
+*/
+inv_error_t inv_get_gyro_sensor(long *data)
+{
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+    if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) {
+        inv_error_t result;
+        unsigned char regs[6];
+        if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) {
+            fifo_obj.cache |= FIFO_CACHE_GYRO;
+            result =
+                inv_serial_read(inv_get_serial_handle(),
+                                inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6,
+                                regs);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            fifo_obj.decoded[REF_RAW + 1] =
+                (((long)regs[0]) << 24) | (((long)regs[1]) << 16);
+            fifo_obj.decoded[REF_RAW + 2] =
+                (((long)regs[2]) << 24) | (((long)regs[3]) << 16);
+            fifo_obj.decoded[REF_RAW + 3] =
+                (((long)regs[4]) << 24) | (((long)regs[5]) << 16);
+
+            // Temperature starts at location 0, Gyro at location 1.
+            fifo_obj.decoded[REF_RAW + 1] =
+                inv_q30_mult(fifo_obj.decoded[REF_RAW + 1],
+                             fifo_scale[REF_RAW + 1]);
+            fifo_obj.decoded[REF_RAW + 2] =
+                inv_q30_mult(fifo_obj.decoded[REF_RAW + 2],
+                             fifo_scale[REF_RAW + 2]);
+            fifo_obj.decoded[REF_RAW + 3] =
+                inv_q30_mult(fifo_obj.decoded[REF_RAW + 3],
+                             fifo_scale[REF_RAW + 3]);
+        }
+        data[0] = fifo_obj.decoded[REF_RAW + 1];
+        data[1] = fifo_obj.decoded[REF_RAW + 2];
+        data[2] = fifo_obj.decoded[REF_RAW + 3];
+    } else {
+        long data2[6];
+        inv_get_gyro_and_accel_sensor(data2);
+        data[0] = data2[0];
+        data[1] = data2[1];
+        data[2] = data2[2];
+    }
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 6-element vector of gyro and accel data
+ *  @param[out] data    6-element vector of gyro and accel data
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_gyro_and_accel_sensor(long *data)
+{
+    int ii;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_RAW_DATA])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) {
+        data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii];
+    }
+
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 3-element vector of external sensor
+ *  @param[out] data    3-element vector of external sensor
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_external_sensor_data(long *data, int size)
+{
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+    int ii;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (ii = 0; ii < size && ii < 6; ii++) {
+        data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii];
+    }
+
+    return INV_SUCCESS;
+#else
+    memset(data, 0, COMPASS_NUM_AXES * sizeof(long));
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+#endif
+}
+
+/** 
+ *  Sends accelerometer data to the FIFO.
+ *
+ *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis
+ *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
+ *            for a subset.
+ *
+ * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *            bit data. Set to zero to remove it from the FIFO.
+ */
+inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 };
+    inv_error_t result;
+    int kk;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL,
+                                 KEY_CFG_12, CONFIG_ACCEL);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
+        fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens;
+    }
+
+    return inv_set_footer();
+}
+
+/** 
+ * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits.
+ *
+ *  @param[in] elements Which of the 4 elements to send. Use INV_ALL for all
+ *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd
+ *             together for a subset.
+ *
+ *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *             bit data. Set to zero to remove it from the FIFO.
+ */
+inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    int_fast8_t kk;
+    inv_error_t result;
+    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4);
+    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA);
+
+    if (accuracy & INV_16_BIT) {
+        regs[0] = DINAF8 + 2;
+    }
+
+    fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy;
+
+    for (kk = 0; kk < 4; ++kk) {
+        if ((elements & 1) == 0)
+            regs[kk + 1] = DINAA0 + 3;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_1, 5, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+/** 
+ * Adds a rolling counter to the fifo packet.  When used with the footer
+ * the data comes out the first time:
+ * 
+ * @code
+ * <data0><data1>...<dataN><PacketNum0><PacketNum1>
+ * @endcode
+ * for every other packet it is
+ *
+ * @code
+ * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1>
+ * @endcode
+ *
+ * This allows for scanning of the fifo for packets
+ * 
+ * @return INV_SUCCESS or error code
+ */
+inv_error_t inv_send_packet_number(uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    unsigned char regs;
+    uint_fast16_t elements;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1);
+    if (elements & 1) {
+        regs = DINA28;
+        fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] =
+            INV_ELEMENT_1 | INV_16_BIT;
+    } else {
+        regs = DINAF8 + 3;
+        fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0;
+    }
+    result = inv_set_mpu_memory(KEY_CFG_23, 1, &regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+/**
+ *  @brief  Send the computed gravity vectors into the FIFO.
+ *          The gravity vectors can be retrieved from the FIFO via 
+ *          inv_get_gravity(), to have the gravitation vector expressed
+ *          in coordinates relative to the body.
+ *
+ *  Gravity is a derived vector derived from the quaternion.
+ *  @param  elements
+ *              the gravitation vectors components bitmask.
+ *              To send all compoents use INV_ALL.
+ *  @param  accuracy
+ *              The number of bits the gravitation vector is expressed 
+ *              into.
+ *              Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *              bit data. 
+ *              Set to zero to remove it from the FIFO.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+
+    result = inv_send_quaternion(accuracy);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+/** Sends gyro data to the FIFO. Gyro data is a 3 length vector
+ *  of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
+ *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
+ *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
+ *            for a subset.
+ *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *             bit data. Set to zero to remove it from the FIFO.
+ */
+inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 };
+    inv_error_t result;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) {
+        regs[0] = DINA90 + 5;
+        result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        regs[0] = DINAF8 + 1;
+        regs[1] = DINA20;
+        regs[2] = DINA28;
+        regs[3] = DINA30;
+    } else {
+        regs[0] = DINA90 + 10;
+        result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        regs[0] = DINAF8 + 1;
+        regs[1] = DINA28;
+        regs[2] = DINA30;
+        regs[3] = DINA38;
+    }
+    result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS,
+                                 KEY_CFG_9, CONFIG_GYROS);
+
+    return inv_set_footer();
+}
+
+/** Sends linear accelerometer data to the FIFO. 
+ *
+ *  Linear accelerometer data is a 3 length vector of 32-bits. It is the 
+ *  acceleration in the body frame with gravity removed.
+ * 
+ * 
+ *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
+ *            them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
+ *            for a subset.
+ *
+ *  NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES
+ *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *             bit data. Set to zero to remove it from the FIFO.
+ */
+inv_error_t inv_send_linear_accel(uint_fast16_t elements,
+                                  uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    unsigned char state = inv_get_state();
+
+    if (state < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    result = inv_send_gravity(elements, accuracy);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_send_accel(elements, accuracy);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+/** Sends linear world accelerometer data to the FIFO. Linear world
+ *  accelerometer data is a 3 length vector of 32-bits. It is the acceleration
+ *  in the world frame with gravity removed. Should be called once after
+ *  inv_dmp_open() and before inv_dmp_start().
+ *
+ *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 
+ *             them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
+ *             for a subset.
+ *  @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *             bit data.
+ */
+inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
+                                           uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+
+    result = inv_send_linear_accel(INV_ALL, accuracy);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_send_quaternion(accuracy);
+
+    return inv_set_footer();
+}
+
+/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector
+ *   of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
+ * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
+ *            bit data.
+ */
+inv_error_t inv_send_quaternion(uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
+        DINA30, DINA38
+    };
+    uint_fast16_t elements, kk;
+    inv_error_t result;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4);
+    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT);
+
+    if (accuracy & INV_16_BIT) {
+        regs[0] = DINAF8 + 2;
+    }
+
+    fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy;
+
+    for (kk = 0; kk < 4; ++kk) {
+        if ((elements & 1) == 0)
+            regs[kk + 1] = DINAA0 + 3;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_8, 5, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+/** Sends raw data to the FIFO. 
+ *  Should be called once after inv_dmp_open() and before inv_dmp_start().
+ *  @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them
+ *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together
+ *            for a subset. The first element is temperature, the next 3 are gyro data,
+ *            and the last 3 accel data.
+ *  @param  accuracy
+ *              The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off.
+ *  @return 0 if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    int result;
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+    unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
+        DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
+        DINAA0 + 3
+    };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (accuracy)
+        accuracy = INV_16_BIT;
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
+
+    if (elements & 1)
+        fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
+    else
+        fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
+    if (elements & 0x7e)
+        fifo_obj.data_config[CONFIG_RAW_DATA] =
+            (0x3f & (elements >> 1)) | INV_16_BIT;
+    else
+        fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
+
+    if (elements & INV_ELEMENT_1) {
+        regs[0] = DINACA;
+    }
+    if (elements & INV_ELEMENT_2) {
+        regs[1] = DINBC4;
+    }
+    if (elements & INV_ELEMENT_3) {
+        regs[2] = DINACC;
+    }
+    if (elements & INV_ELEMENT_4) {
+        regs[3] = DINBC6;
+    }
+    if (elements & INV_ELEMENT_5) {
+        regs[4] = DINBC0;
+    }
+    if (elements & INV_ELEMENT_6) {
+        regs[5] = DINAC8;
+    }
+    if (elements & INV_ELEMENT_7) {
+        regs[6] = DINBC2;
+    }
+    result = inv_set_mpu_memory(KEY_CFG_15, 7, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+
+#else
+    INVENSENSE_FUNC_START;
+    unsigned char regs[4] = { DINAA0 + 3,
+        DINAA0 + 3,
+        DINAA0 + 3,
+        DINAA0 + 3
+    };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (accuracy)
+        accuracy = INV_16_BIT;
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
+
+    if (elements & 0x03) {
+        elements |= 0x03;
+        regs[0] = DINA20;
+    }
+    if (elements & 0x0C) {
+        elements |= 0x0C;
+        regs[1] = DINA28;
+    }
+    if (elements & 0x30) {
+        elements |= 0x30;
+        regs[2] = DINA30;
+    }
+    if (elements & 0x40) {
+        elements |= 0xC0;
+        regs[3] = DINA38;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_15, 4, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    if (elements & 0x01)
+        fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
+    else
+        fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
+    if (elements & 0xfe)
+        fifo_obj.data_config[CONFIG_RAW_DATA] =
+            (0x7f & (elements >> 1)) | INV_16_BIT;
+    else
+        fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
+
+    return inv_set_footer();
+#endif
+}
+
+/** Sends raw external data to the FIFO. 
+ *  Should be called once after inv_dmp_open() and before inv_dmp_start().
+ *  @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
+ *            or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
+ *            for a subset. 
+ *  @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data.
+ *            Sending and Stop sending are reference counted, so data actually
+ *            stops when the reference reaches zero.
+ */
+inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
+                                          uint_fast16_t accuracy)
+{
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+    int result;
+    unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3,
+                              DINAA0 + 3, DINAA0 + 3,
+                              DINAA0 + 3, DINAA0 + 3 };
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (accuracy)
+        accuracy = INV_16_BIT;
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6);
+
+    if (elements)
+        fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT;
+    else
+        fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0;
+
+    if (elements & INV_ELEMENT_1) {
+        regs[0] = DINBC2;
+    }
+    if (elements & INV_ELEMENT_2) {
+        regs[1] = DINACA;
+    }
+    if (elements & INV_ELEMENT_3) {
+        regs[2] = DINBC4;
+    }
+    if (elements & INV_ELEMENT_4) {
+        regs[3] = DINBC0;
+    }
+    if (elements & INV_ELEMENT_5) {
+        regs[4] = DINAC8;
+    }
+    if (elements & INV_ELEMENT_6) {
+        regs[5] = DINACC;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+
+#else
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;    // Feature not supported
+#endif
+}
+
+/**
+ *  @brief  Send the Quantized Acceleromter data into the FIFO.  The data can be
+ *          retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel().
+ *
+ *  To be useful this should be set to fifo_rate + 1 if less than
+ *  INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work.
+ *
+ *  @param  elements
+ *              the components bitmask.
+ *              To send all compoents use INV_ALL.
+ *
+ *  @param  accuracy
+ *              Use INV_32_BIT for 32-bit data or INV_16_BIT for 
+ *              16-bit data.
+ *              Set to zero to remove it from the FIFO.
+ *
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
+                                     uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
+        DINA30, DINA38
+    };
+    unsigned char regs2[4] = { DINA20, DINA28,
+        DINA30, DINA38
+    };
+    inv_error_t result;
+    int_fast8_t kk;
+    int_fast8_t ii;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8);
+
+    if (elements) {
+        fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT;
+    } else {
+        fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0;
+    }
+
+    for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) {
+        fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0;
+        for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
+            fifo_obj.decoded_accel[kk][ii] = 0;
+        }
+    }
+
+    for (kk = 0; kk < 4; ++kk) {
+        if ((elements & 1) == 0)
+            regs[kk + 1] = DINAA0 + 3;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    for (kk = 0; kk < 4; ++kk) {
+        if ((elements & 1) == 0)
+            regs2[kk] = DINAA0 + 3;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return inv_set_footer();
+}
+
+inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy)
+{
+    INVENSENSE_FUNC_START;
+    int_fast8_t kk;
+    unsigned char regs[3] = { DINA28, DINA30, DINA38 };
+    inv_error_t result;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (accuracy) {
+        accuracy = INV_32_BIT;
+    }
+
+    elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3);
+    accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS);
+
+    fifo_obj.data_config[CONFIG_EIS] = elements | accuracy;
+
+    for (kk = 0; kk < 3; ++kk) {
+        if ((elements & 1) == 0)
+            regs[kk] = DINAA0 + 7;
+        elements >>= 1;
+    }
+
+    result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs);
+
+    return inv_set_footer();
+}
+
+/** 
+ * @brief       Returns 3-element vector of accelerometer data in body frame.
+ *
+ * @param[out]  data    3-element vector of accelerometer data in body frame.
+ *                      One gee = 2^16.
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_accel(long *data)
+{
+    int kk;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if ((!fifo_obj.data_config[CONFIG_ACCEL] &&
+         (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR))
+        ||
+        (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
+         !inv_accel_present()))
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_ACCEL + kk];
+    }
+
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 4-element quaternion vector derived from 6-axis or 
+ *  9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is
+ *  gyros, accel and compass.
+ *
+ *  @param[out] data    4-element quaternion vector. One is scaled to 2^30.
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_quaternion(long *data)
+{
+    int kk;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_QUAT])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < 4; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_QUATERNION + kk];
+    }
+
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 4-element quaternion vector derived from 6 
+ *              axis sensors (gyros and accels).
+ *  @param[out] data    
+ *                  4-element quaternion vector. One is scaled to 2^30.
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_6axis_quaternion(long *data)
+{
+    int kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_QUAT])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < 4; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk];
+    }
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_get_relative_quaternion(long *data)
+{
+    if (data == NULL)
+        return INV_ERROR;
+    data[0] = inv_obj.relative_quat[0];
+    data[1] = inv_obj.relative_quat[1];
+    data[2] = inv_obj.relative_quat[2];
+    data[3] = inv_obj.relative_quat[3];
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  Returns 3-element vector of gyro data in body frame.
+ *  @param[out] data    
+ *              3-element vector of gyro data in body frame 
+ *              with gravity removed. One degree per second = 2^16.
+ *  @return 0 on success or an error code.
+ */
+inv_error_t inv_get_gyro(long *data)
+{
+    int kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (fifo_obj.data_config[CONFIG_GYROS]) {
+        for (kk = 0; kk < 3; ++kk) {
+            data[kk] = fifo_obj.decoded[REF_GYROS + kk];
+        }
+        return INV_SUCCESS;
+    } else {
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+    }
+}
+
+/**
+ *  @brief  Get the 3-element gravity vector from the FIFO expressed
+ *          in coordinates relative to the body frame.
+ *  @param  data    
+ *              3-element vector of gravity in body frame.
+ *  @return 0 on success or an error code.
+ */
+inv_error_t inv_get_gravity(long *data)
+{
+    long quat[4];
+    int ii;
+    inv_error_t result;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_QUAT])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) {
+        fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY;
+
+        // Compute it from Quaternion
+        result = inv_get_quaternion(quat);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        data[0] =
+            inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
+        data[1] =
+            inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
+        data[2] =
+            (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) -
+            1073741824L;
+
+        for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
+            data[ii] >>= 14;
+            fifo_obj.gravity_cache[ii] = data[ii];
+        }
+    } else {
+        data[0] = fifo_obj.gravity_cache[0];
+        data[1] = fifo_obj.gravity_cache[1];
+        data[2] = fifo_obj.gravity_cache[2];
+    }
+
+    return INV_SUCCESS;
+}
+
+/** 
+* @brief        Sets the filter coefficent used for computing the acceleration
+*               bias which is used to compute linear acceleration.
+* @param[in] coef   Fitler coefficient. 0. means no filter, a small number means
+*                   a small cutoff frequency with an increasing number meaning 
+*                   an increasing cutoff frequency.
+*/
+inv_error_t inv_set_linear_accel_filter_coef(float coef)
+{
+    fifo_obj.acc_filter_coef = coef;
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 3-element vector of accelerometer data in body frame
+ *              with gravity removed.
+ *  @param[out] data    3-element vector of accelerometer data in body frame
+ *                      with gravity removed. One g = 2^16.
+ *  @return     0 on success or an error code. data unchanged on error.
+ */
+inv_error_t inv_get_linear_accel(long *data)
+{
+    int kk;
+    long grav[3];
+    long la[3];
+    inv_error_t result;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    result = inv_get_gravity(grav);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_get_accel(la);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) {
+        fifo_obj.cache |= FIFO_CACHE_ACC_BIAS;
+
+        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+            long x;
+            x = la[kk] - grav[kk];
+            fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef +
+                                                fifo_obj.acc_bias_filt[kk] *
+                                                (1.f -
+                                                 fifo_obj.acc_filter_coef));
+            data[kk] = x - fifo_obj.acc_bias_filt[kk];
+        }
+    } else {
+        for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+            data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk];
+        }
+    }
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 3-element vector of accelerometer data in world frame
+ *              with gravity removed.
+ *  @param[out] data    3-element vector of accelerometer data in world frame
+ *                      with gravity removed. One g = 2^16.
+ *  @return     0 on success or an error code.
+ */
+inv_error_t inv_get_linear_accel_in_world(long *data)
+{
+    int kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+    if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) {
+        long wtemp[4], qi[4], wtemp2[4];
+        wtemp[0] = 0;
+        inv_get_linear_accel(&wtemp[1]);
+        inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2);
+        inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi);
+        inv_q_mult(wtemp2, qi, wtemp);
+        for (kk = 0; kk < 3; ++kk) {
+            data[kk] = wtemp[kk + 1];
+        }
+        return INV_SUCCESS;
+    } else {
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+    }
+}
+
+/**
+ *  @brief      Returns 4-element vector of control data.
+ *  @param[out] data    4-element vector of control data.
+ *  @return     0 for succes or an error code.
+ */
+inv_error_t inv_get_cntrl_data(long *data)
+{
+    int kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_CONTROL_DATA])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < 4; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_CONTROL + kk];
+    }
+
+    return INV_SUCCESS;
+
+}
+
+/**
+ *  @brief      Returns 3-element vector of EIS shfit data
+ *  @param[out] data    3-element vector of EIS shift data.
+ *  @return     0 for succes or an error code.
+ */
+inv_error_t inv_get_eis(long *data)
+{
+    int kk;
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_EIS])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < 3; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_EIS + kk];
+    }
+
+    return INV_SUCCESS;
+
+}
+
+/** 
+ *  @brief      Returns 3-element vector of accelerometer data in body frame.
+ *  @param[out] data    3-element vector of accelerometer data in body frame in g's.
+ *  @return     0 for success or an error code.
+ */
+inv_error_t inv_get_accel_float(float *data)
+{
+    long lData[3];
+    int kk;
+    int result;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    result = inv_get_accel(lData);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
+        data[kk] = lData[kk] / 65536.0f;
+    }
+
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief      Returns 4-element quaternion vector.
+ *  @param[out] data    4-element quaternion vector.
+ *  @return     0 on success, an error code otherwise.
+ */
+inv_error_t inv_get_quaternion_float(float *data)
+{
+    int kk;
+
+    if (data == NULL)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    if (!fifo_obj.data_config[CONFIG_QUAT])
+        return INV_ERROR_FEATURE_NOT_ENABLED;
+
+    for (kk = 0; kk < 4; ++kk) {
+        data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f;
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   Command the MPU to put data in the FIFO at a particular rate.
+ *
+ *          The DMP will add fifo entries every fifoRate + 1 MPU cycles.  For
+ *          example if the MPU is running at 200Hz the following values apply:
+ *
+ *          <TABLE>
+ *          <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR>
+ *          <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR>
+ *          <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR>
+ *          <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR>
+ *          <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR>
+ *          <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR>
+ *          <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR>
+ *          </TABLE>
+ *
+ *          Note: if the DMP is running, (state == INV_STATE_DMP_STARTED)
+ *          then inv_run_state_callbacks() will be called to allow features
+ *          that depend upon fundamental constants to be updated.
+ *
+ *  @pre    inv_dmp_open() 
+ *          @ifnot MPL_MF 
+ *              or inv_open_low_power_pedometer() 
+ *              or inv_eis_open_dmp()
+ *          @endif
+ *          and inv_dmp_start() 
+ *          must <b>NOT</b> have been called.
+ *
+ * @param   fifoRate    Divider value - 1.  Output rate is 
+ *          (DMP Sample Rate) / (fifoRate + 1).
+ *
+ * @return  INV_SUCCESS if successful, ML error code on any failure.
+ */
+inv_error_t inv_set_fifo_rate(unsigned short fifoRate)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char regs[2];
+    unsigned char state;
+    inv_error_t result = INV_SUCCESS;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    state = inv_get_state();
+    if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    fifo_obj.fifo_rate = fifoRate;
+
+    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
+
+        regs[0] = (unsigned char)((fifoRate >> 8) & 0xff);
+        regs[1] = (unsigned char)(fifoRate & 0xff);
+
+        result = inv_set_mpu_memory(KEY_D_0_22, 2, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        fifo_obj.sample_step_size_ms = 
+            (unsigned short)(((long)fifoRate + 1) *
+                             (inv_mpu_get_sampling_period_us
+                              (mldl_cfg)) / 1000L);
+
+        if (state == INV_STATE_DMP_STARTED)
+            result = inv_run_state_callbacks(state);
+    } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) {
+        struct ext_slave_config config;
+        long data;
+        config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
+        config.len = sizeof(long);
+        config.apply = (state == INV_STATE_DMP_STARTED);
+        config.data = &data;
+        data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1);
+
+        /* Ask for the same frequency */
+        result = inv_mpu_config_accel(mldl_cfg,
+                                      inv_get_serial_handle(),
+                                      inv_get_serial_handle(), &config);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_mpu_get_accel_config(mldl_cfg,
+                                          inv_get_serial_handle(),
+                                          inv_get_serial_handle(), &config);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        if(FIFO_DEBUG)
+            MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000);
+        /* Record the actual frequency granted odr is in mHz */
+        fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data);
+    }
+    return result;
+}
+
+/**
+ * @brief   Retrieve the current FIFO update divider - 1.
+ *          See inv_set_fifo_rate() for how this value is used.
+ *
+ *          The fifo rate when there is no fifo is the equivilent divider when
+ *          derived from the value set by SetSampleSteSizeMs()
+ *          
+ * @return  The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error.
+ */
+unsigned short inv_get_fifo_rate(void)
+{
+    return fifo_obj.fifo_rate;
+}
+
+/**
+ * @brief   Returns the step size for quaternion type data.
+ *
+ * Typically the data rate for each FIFO packet. When the gryos are sleeping
+ * this value will return the last value set by SetSampleStepSizeMs()
+ *
+ * @return  step size for quaternion type data
+ */
+int_fast16_t inv_get_sample_step_size_ms(void)
+{
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
+        return (fifo_obj.fifo_rate + 1) *
+            (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000);
+    else
+        return fifo_obj.sample_step_size_ms;
+}
+
+/**
+ * @brief   Returns the step size for quaternion type data.
+ *
+ * Typically the data rate for each FIFO packet. When the gryos are sleeping
+ * this value will return the last value set by SetSampleStepSizeMs()
+ *
+ * @return  step size for quaternion type data
+ */
+int_fast16_t inv_get_sample_frequency(void)
+{
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
+        return (inv_mpu_get_sampling_rate_hz(mldl_cfg) /
+                (fifo_obj.fifo_rate + 1));
+    else
+        return (1000 / fifo_obj.sample_step_size_ms);
+}
+
+/** 
+ *  @brief  The gyro data magnitude squared : 
+ *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
+ *  @return the computed magnitude squared output of the gyroscope.
+ */
+unsigned long inv_get_gyro_sum_of_sqr(void)
+{
+    unsigned long gmag = 0;
+    long temp;
+    int kk;
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = fifo_obj.decoded[REF_GYROS + kk] >>
+            (16 - (GYRO_MAG_SQR_SHIFT / 2));
+        gmag += temp * temp;
+    }
+
+    return gmag;
+}
+
+/** 
+ *  @brief  The gyro data magnitude squared:
+ *          (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT.
+ *  @return the computed magnitude squared output of the accelerometer.
+ */
+unsigned long inv_accel_sum_of_sqr(void)
+{
+    unsigned long amag = 0;
+    long temp;
+    int kk;
+    long accel[3];
+    inv_error_t result;
+
+    result = inv_get_accel(accel);
+    if (INV_SUCCESS != result) {
+        return 0;
+    }
+
+    for (kk = 0; kk < 3; ++kk) {
+        temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2));
+        amag += temp * temp;
+    }
+    return amag;
+}
+
+/**
+ *  @internal
+ */
+void inv_override_quaternion(float *q)
+{
+    int kk;
+    for (kk = 0; kk < 4; ++kk) {
+        fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30));
+    }
+}
+
+/**
+ * @internal
+ * @brief   This registers a function to be called for each set of 
+ *          gyro/quaternion/rotation matrix/etc output.
+ * @param[in] func The callback function to register
+ * @param[in] priority The unique priority number of the callback. Lower numbers
+ *            are called first.
+ * @return  error code.
+ */
+inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    int kk, nn;
+
+    result = inv_lock_mutex(fifo_rate_obj.mutex);
+    if (INV_SUCCESS != result) {
+        return result;
+    }
+    // Make sure we haven't registered this function already
+    // Or used the same priority
+    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
+        if ((fifo_rate_obj.fifo_process_cb[kk] == func) ||
+            (fifo_rate_obj.priority[kk] == priority)) {
+            inv_unlock_mutex(fifo_rate_obj.mutex);
+            return INV_ERROR_INVALID_PARAMETER;
+        }
+    }
+
+    // Make sure we have not filled up our number of allowable callbacks
+    if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) {
+        kk = 0;
+        if (fifo_rate_obj.num_cb != 0) {
+            // set kk to be where this new callback goes in the array
+            while ((kk < fifo_rate_obj.num_cb) &&
+                   (fifo_rate_obj.priority[kk] < priority)) {
+                kk++;
+            }
+            if (kk != fifo_rate_obj.num_cb) {
+                // We need to move the others
+                for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) {
+                    fifo_rate_obj.fifo_process_cb[nn] =
+                        fifo_rate_obj.fifo_process_cb[nn - 1];
+                    fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1];
+                }
+            }
+        }
+        // Add new callback
+        fifo_rate_obj.fifo_process_cb[kk] = func;
+        fifo_rate_obj.priority[kk] = priority;
+        fifo_rate_obj.num_cb++;
+    } else {
+        result = INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    inv_unlock_mutex(fifo_rate_obj.mutex);
+    return result;
+}
+
+/**
+ * @internal
+ * @brief   This unregisters a function to be called for each set of 
+ *          gyro/quaternion/rotation matrix/etc output.
+ * @return  error code.
+ */
+inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func)
+{
+    INVENSENSE_FUNC_START;
+    int kk, jj;
+    inv_error_t result;
+
+    result = inv_lock_mutex(fifo_rate_obj.mutex);
+    if (INV_SUCCESS != result) {
+        return result;
+    }
+    // Make sure we haven't registered this function already
+    result = INV_ERROR_INVALID_PARAMETER;
+    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
+        if (fifo_rate_obj.fifo_process_cb[kk] == func) {
+            for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) {
+                fifo_rate_obj.fifo_process_cb[jj - 1] =
+                    fifo_rate_obj.fifo_process_cb[jj];
+                fifo_rate_obj.priority[jj - 1] =
+                    fifo_rate_obj.priority[jj];
+            }
+            fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL;
+            fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0;
+            fifo_rate_obj.num_cb--;
+            result = INV_SUCCESS;
+            break;
+        }
+    }
+
+    inv_unlock_mutex(fifo_rate_obj.mutex);
+    return result;
+
+}
+#ifdef UMPL
+bool bFIFIDataAvailable = FALSE;
+bool isUmplDataInFIFO(void)
+{
+    return bFIFIDataAvailable;
+}
+void setUmplDataInFIFOFlag(bool flag)
+{
+    bFIFIDataAvailable = flag;
+}
+#endif
+inv_error_t inv_run_fifo_rate_processes(void)
+{
+    int kk;
+    inv_error_t result, result2;
+
+    result = inv_lock_mutex(fifo_rate_obj.mutex);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("MLOsLockMutex returned %d\n", result);
+        return result;
+    }
+    // User callbacks take priority over the fifo_process_cb callback
+    if (fifo_obj.fifo_process_cb)
+        fifo_obj.fifo_process_cb();
+
+    for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
+        if (fifo_rate_obj.fifo_process_cb[kk]) {
+            result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj);
+            if (result == INV_SUCCESS)
+#ifdef UMPL		 
+	 setUmplDataInFIFOFlag(TRUE);
+#endif
+                result = result2;
+        }
+    }
+
+    inv_unlock_mutex(fifo_rate_obj.mutex);
+    return result;
+}
+
+/*********************/
+         /** \}*//* defgroup */
+/*********************/
diff --git a/60xx/mlsdk/mllite/mlFIFO.h b/60xx/mlsdk/mllite/mlFIFO.h
new file mode 100644
index 0000000..2114eb3
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlFIFOHW.c b/60xx/mlsdk/mllite/mlFIFOHW.c
new file mode 100644
index 0000000..7a77e66
--- /dev/null
+++ b/60xx/mlsdk/mllite/mlFIFOHW.c
@@ -0,0 +1,328 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *******************************************************************************/
+
+/** 
+ *  @defgroup MLFIFO_HW 
+ *  @brief  Motion Library - FIFO HW Driver.
+ *          Provides facilities to interact with the FIFO.
+ *
+ *  @{
+ *      @file   mlFIFOHW.c
+ *      @brief  The Motion Library Fifo Hardware Layer.
+ *
+ */
+
+#include <string.h>
+
+#include "mpu.h"
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+#    include "mpu6050a2.h"
+#elif defined CONFIG_MPU_SENSORS_MPU6050B1
+#    include "mpu6050b1.h"
+#elif defined CONFIG_MPU_SENSORS_MPU3050
+#  include "mpu3050.h"
+#else
+#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
+#endif
+#include "mlFIFOHW.h"
+#include "ml.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+
+#include "mlsl.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-fifo"
+
+/*
+    Defines
+*/
+
+#define _fifoDebug(x)           //{x}
+
+/*
+    Typedefs
+*/
+
+struct fifo_hw_obj {
+    short fifoCount;
+    inv_error_t fifoError;
+    unsigned char fifoOverflow;
+    unsigned char fifoResetOnOverflow;
+};
+
+/*
+    Global variables
+*/
+const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
+
+/*
+    Static variables
+*/
+static struct fifo_hw_obj fifo_objHW;
+
+/*
+    Definitions
+*/
+
+/**
+ *  @brief  Initializes the internal FIFO data structure.
+ */
+void inv_init_fifo_hardare(void)
+{
+    memset(&fifo_objHW, 0, sizeof(fifo_objHW));
+    fifo_objHW.fifoResetOnOverflow = TRUE;
+}
+
+/**
+ *  @internal
+ *  @brief  used to get the FIFO data.
+ *  @param  length  
+ *              Number of bytes to read from the FIFO.
+ *  @param  buffer  
+ *              the bytes of FIFO data.
+ *              Note that this buffer <b>must</b> be large enough
+ *              to store and additional trailing FIFO footer when 
+ *              expected.  The callers must make sure enough space
+ *              is allocated.
+ *  @return number of valid bytes of data.
+**/
+uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    uint_fast16_t inFifo;
+    uint_fast16_t toRead;
+    int_fast8_t kk;
+
+    toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
+    /*---- make sure length is correct ----*/
+    if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
+        fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
+        return 0;
+    }
+
+    result = inv_get_fifo_length(&inFifo);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+    // fifo_objHW.fifoCount is the footer size left in the buffer, or 
+    //      0 if this is the first time reading the fifo since it was reset
+    if (inFifo < length + fifo_objHW.fifoCount) {
+        fifo_objHW.fifoError = INV_SUCCESS;
+        return 0;
+    }
+    // if a trailing fifo count is expected - start storing data 2 bytes before
+    result =
+        inv_read_fifo(fifo_objHW.fifoCount >
+                      0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+    // Make sure the fifo didn't overflow before or during the read
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
+    if (INV_SUCCESS != result) {
+        fifo_objHW.fifoError = result;
+        return 0;
+    }
+
+    if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
+        MPL_LOGV("Resetting Fifo : Overflow\n");
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
+        return 0;
+    }
+
+    /* Check the Footer value to give us a chance at making sure data 
+     * didn't get corrupted */
+    for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
+        if (buffer[kk] != gFifoFooter[kk]) {
+            MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
+                     buffer[0], buffer[1]);
+            _fifoDebug(char out[200];
+                       MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
+                       sprintf(out, "0x");
+                       for (kk = 0; kk < (int)toRead; kk++) {
+                       sprintf(out, "%s%02X", out, buffer[kk]);}
+                       MPL_LOGW("%s\n", out);)
+                inv_reset_fifo();
+            fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
+            return 0;
+        }
+    }
+
+    if (fifo_objHW.fifoCount == 0) {
+        fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
+    }
+
+    return length - FIFO_FOOTER_SIZE;
+}
+
+/**
+ *  @brief  Used to query the status of the FIFO.
+ *  @return INV_SUCCESS if the fifo is OK. An error code otherwise.
+**/
+inv_error_t inv_get_fifo_status(void)
+{
+    inv_error_t fifoError = fifo_objHW.fifoError;
+    fifo_objHW.fifoError = 0;
+    return fifoError;
+}
+
+/** 
+ * @internal
+ * @brief   Get the length from the fifo
+ * 
+ * @param[out] len amount of data currently stored in the fifo.
+ * 
+ * @return INV_SUCCESS or non-zero error code.
+**/
+inv_error_t inv_get_fifo_length(uint_fast16_t * len)
+{
+    INVENSENSE_FUNC_START;
+    unsigned char fifoBuf[2];
+    inv_error_t result;
+
+    if (NULL == len)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    /*---- read the 2 'count' registers and 
+      burst read the data from the FIFO ----*/
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_FIFO_COUNTH, 2, fifoBuf);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("ReadBurst failed %d\n", result);
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
+        *len = 0;
+        return result;
+    }
+
+    *len = (uint_fast16_t) (fifoBuf[0] << 8);
+    *len += (uint_fast16_t) (fifoBuf[1]);
+    return result;
+}
+
+/**
+ *  @brief  inv_get_fifo_count is used to get the number of bytes left in the FIFO.
+ *          This function returns the stored value and does not access the hardware.  
+ *          See inv_get_fifo_length().
+ *  @return the number of bytes left in the FIFO
+**/
+short inv_get_fifo_count(void)
+{
+    return fifo_objHW.fifoCount;
+}
+
+/** 
+ *  @internal
+ *  @brief  Read data from the fifo
+ * 
+ *  @param[out] data Location to store the date read from the fifo
+ *  @param[in] len   Amount of data to read out of the fifo
+ * 
+ *  @return INV_SUCCESS or non-zero error code
+**/
+inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result;
+    result = inv_serial_read_fifo(inv_get_serial_handle(),
+                                  inv_get_mpu_slave_addr(),
+                                  (unsigned short)len, data);
+    if (INV_SUCCESS != result) {
+        MPL_LOGE("inv_serial_readBurst failed %d\n", result);
+        inv_reset_fifo();
+        fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  Clears the FIFO status and its content. 
+ *  @note   Halt the DMP writing into the FIFO for the time 
+ *          needed to reset the FIFO.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_reset_fifo(void)
+{
+    INVENSENSE_FUNC_START;
+    int len = FIFO_HW_SIZE;
+    unsigned char fifoBuf[2];
+    unsigned char tries = 0;
+    unsigned char userCtrlReg;
+    inv_error_t result;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    fifo_objHW.fifoCount = 0;
+    if (mldl_cfg->gyro_is_suspended)
+        return INV_SUCCESS;
+
+    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                             MPUREG_USER_CTRL, 1, &userCtrlReg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    while (len != 0 && tries < 6) {
+        result =
+            inv_serial_single_write(inv_get_serial_handle(),
+                                    inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
+                                    ((userCtrlReg & (~BIT_FIFO_EN)) |
+                                     BIT_FIFO_RST));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result =
+            inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
+                            MPUREG_FIFO_COUNTH, 2, fifoBuf);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
+        tries++;
+    }
+
+    result =
+        inv_serial_single_write(inv_get_serial_handle(),
+                                inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
+                                userCtrlReg);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+**/
diff --git a/60xx/mlsdk/mllite/mlFIFOHW.h b/60xx/mlsdk/mllite/mlFIFOHW.h
new file mode 100644
index 0000000..6f70185
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlMathFunc.c b/60xx/mlsdk/mllite/mlMathFunc.c
new file mode 100644
index 0000000..0d8e7ec
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlMathFunc.h b/60xx/mlsdk/mllite/mlMathFunc.h
new file mode 100644
index 0000000..70fa9f4
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlSetGyroBias.c b/60xx/mlsdk/mllite/mlSetGyroBias.c
new file mode 100644
index 0000000..bd14d2e
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlSetGyroBias.h b/60xx/mlsdk/mllite/mlSetGyroBias.h
new file mode 100644
index 0000000..e56f18b
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/ml_mputest.c b/60xx/mlsdk/mllite/ml_mputest.c
new file mode 100644
index 0000000..d7fc608
--- /dev/null
+++ b/60xx/mlsdk/mllite/ml_mputest.c
@@ -0,0 +1,184 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ *
+ * $Id: ml_mputest.c 5641 2011-06-14 02:10:02Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ *  @defgroup MPU_SELF_TEST
+ *  @brief  C wrapper to integrate the MPU Self Test wrapper in MPL.
+ *          Provides ML name compliant naming and an additional API that
+ *          automates the suspension of normal MPL operations, runs the test,
+ *          and resume.
+ *
+ *  @{
+ *      @file   ml_mputest.c
+ *      @brief  C wrapper to integrate the MPU Self Test wrapper in MPL.
+ *              The main logic of the test and APIs can be found in mputest.c
+ */
+
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include "ml_mputest.h"
+
+#include "mlmath.h"
+#include "mlinclude.h"
+#include "ml.h"
+#include "mlstates.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Globals
+*/
+extern struct mldl_cfg *mputestCfgPtr;
+extern signed char g_z_sign;
+
+/*
+    Prototypes
+*/
+extern inv_error_t inv_factory_calibrate(void *mlsl_handle,
+                                         uint_fast8_t provide_result);
+
+/**
+ *  @brief  An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
+ *          See inv_factory_calibrate() function for more details.
+ *
+ *  @pre    inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
+ *          data structure.
+ *          On Windows, SetupPlatform() is also a madatory pre condition to
+ *          ensure the accelerometer is properly configured before running the
+ *          test.
+ *
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  provide_result
+ *              If 1:
+ *              perform and analyze the offset, drive frequency, and noise
+ *              calculation and compare it against set thresholds. Report
+ *              also the final result using a bit-mask like error code as
+ *              described in the inv_test_gyro_xxxx() functions.
+ *              When 0:
+ *              skip the noise and drive frequency calculation  and pass/fail
+ *              assessment. It simply calculates the gyro and accel biases.
+ *              NOTE: for MPU6050 devices, this parameter is currently
+ *              ignored.
+ *
+ *  @return INV_SUCCESS or first non-zero error code otherwise.
+ */
+inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
+                                            unsigned char provide_result)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t firstError = INV_SUCCESS;
+    inv_error_t result;
+    unsigned char initState = inv_get_state();
+
+    if (initState < INV_STATE_DMP_OPENED) {
+        MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
+        return INV_ERROR_SM_IMPROPER_STATE;
+    }
+
+    /* obtain a pointer to the 'struct mldl_cfg' data structure. */
+    mputestCfgPtr = inv_get_dl_config();
+
+    if(initState == INV_STATE_DMP_STARTED) {
+        result = inv_dmp_stop();
+        ERROR_CHECK_FIRST(firstError, result);
+    }
+
+    result = inv_factory_calibrate(mlsl_handle, provide_result);
+    ERROR_CHECK_FIRST(firstError, result);
+
+    if(initState == INV_STATE_DMP_STARTED) {
+        result = inv_dmp_start();
+        ERROR_CHECK_FIRST(firstError, result);
+    }
+
+    return firstError;
+}
+
+/**
+ *  @brief  Runs the MPU test at MPL runtime.
+ *          If the DMP is operating, stops the DMP temporarely,
+ *          runs the MPU Self Test, and re-starts the DMP.
+ *
+ *  @return INV_SUCCESS or a non-zero error code otherwise.
+ */
+inv_error_t inv_self_test_run(void)
+{
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+    return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE);
+#else
+    return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
+#endif
+}
+
+/**
+ *  @brief  Runs the MPU test for bias correction only at MPL runtime.
+ *          If the DMP is operating, stops the DMP temporarely,
+ *          runs the bias calculation routines, and re-starts the DMP.
+ *
+ *  @return INV_SUCCESS or a non-zero error code otherwise.
+ */
+inv_error_t inv_self_test_bias_only(void)
+{
+    return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
+}
+
+/**
+ *  @brief  Set the orientation of the acceleroemter Z axis as it will be
+ *          expected when running the MPU Self Test.
+ *          Specifies the orientation of the accelerometer Z axis : Z axis
+ *          pointing upwards or downwards.
+ *  @param  z_sign
+ *              The sign of the accelerometer Z axis; valid values are +1 and
+ *              -1 for +Z and -Z respectively.  Any other value will cause the
+ *              setting to be ignored and an error code to be returned.
+ *  @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign)
+{
+    if (z_sign != +1 && z_sign != -1) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    g_z_sign = z_sign;
+    return INV_SUCCESS;
+}
+
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ *  @}
+ */
+
diff --git a/60xx/mlsdk/mllite/ml_mputest.h b/60xx/mlsdk/mllite/ml_mputest.h
new file mode 100644
index 0000000..705d3cc
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/ml_stored_data.c b/60xx/mlsdk/mllite/ml_stored_data.c
new file mode 100644
index 0000000..9107fe2
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/ml_stored_data.h b/60xx/mlsdk/mllite/ml_stored_data.h
new file mode 100644
index 0000000..74c2b7c
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlarray.c b/60xx/mlsdk/mllite/mlarray.c
new file mode 100644
index 0000000..6ae85e0
--- /dev/null
+++ b/60xx/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)
+{
+
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    /* not implemented. old (invalid) implementation:
+       if ( inv_get_state() < INV_STATE_DMP_OPENED )
+       return INV_ERROR_SM_IMPROPER_STATE;
+
+       if (NULL == data) {
+       return INV_ERROR_INVALID_PARAMETER;
+       }
+       data[0] = inv_obj.ang_v_body[0];
+       data[1] = inv_obj.ang_v_body[1];
+       data[2] = inv_obj.ang_v_body[2];
+
+       return result;
+     */
+}
+
+/**
+ *  @brief  inv_get_euler_angles is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          Euler angles may follow various conventions. This function is equivelant
+ *          to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more
+ *          information.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_euler_angles(long *data)
+{
+    return inv_get_euler_angles_x(data);
+}
+
+/**
+ *  @brief  inv_get_euler_angles_x is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          Euler angles are returned according to the X convention.
+ *          This is typically the convention used for mobile devices where the X
+ *          axis is the width of the screen, Y axis is the height, and Z the
+ *          depth. In this case roll is defined as the rotation around the X
+ *          axis of the device.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>Y axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
+ *          </TABLE>
+ *
+ *          Values are scaled where 1.0 = 2^16.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_x(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[6];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (long)((float)
+               (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) *
+               65536L);
+    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
+    data[2] =
+        (long)((float)
+               (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) *
+               65536L);
+    return result;
+}
+
+/**
+ *  @brief  inv_get_euler_angles_y is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          Euler angles are returned according to the Y convention.
+ *          This convention is typically used in augmented reality applications,
+ *          where roll is defined as the rotation around the axis along the
+ *          height of the screen of a mobile device, namely the Y axis.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Y axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
+ *          </TABLE>
+ *
+ *          Values are scaled where 1.0 = 2^16.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_y(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[7];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (long)((float)
+               (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) *
+               65536L);
+    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
+    data[2] =
+        (long)((float)
+               (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) *
+               65536L);
+    return result;
+}
+
+/**  @brief  inv_get_euler_angles_z is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          This convention is mostly used in application involving the use
+ *          of a camera, typically placed on the back of a mobile device, that
+ *          is along the Z axis.  In this convention roll is defined as the
+ *          rotation around the Z axis.
+ *          Euler angles are returned according to the Y convention.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Z axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Y axis                </TD></TR>
+ *          </TABLE>
+ *
+ *          Values are scaled where 1.0 = 2^16.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+
+inv_error_t inv_get_euler_angles_z(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[8];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (long)((float)
+               (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) *
+               65536L);
+    data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
+    data[2] =
+        (long)((float)
+               (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) *
+               65536L);
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_temp_slope is used to get is used to get the temperature
+ *          compensation algorithm's estimate of the gyroscope bias temperature
+ *          coefficient.
+ *          The argument array elements are ordered X,Y,Z.
+ *          Values are in units of dps per deg C (degrees per second per degree
+ *          Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs.
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_gyro_temp_slope(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
+        data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f);
+        data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f);
+        data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f);
+    } else {
+        data[0] = inv_obj.temp_slope[0];
+        data[1] = inv_obj.temp_slope[1];
+        data[2] = inv_obj.temp_slope[2];
+    }
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_bias is used to get the gyroscope biases.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are scaled such that 1 dps = 2^16 LSBs.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_gyro_bias(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    data[0] = inv_obj.gyro_bias[0];
+    data[1] = inv_obj.gyro_bias[1];
+    data[2] = inv_obj.gyro_bias[2];
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_accel_bias is used to get the accelerometer baises.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are scaled such that 1 g (gravity) = 2^16 LSBs.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_accel_bias(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    data[0] = inv_obj.accel_bias[0];
+    data[1] = inv_obj.accel_bias[1];
+    data[2] = inv_obj.accel_bias[2];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_bias is used to get Magnetometer Bias
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_bias(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    data[0] =
+        inv_obj.compass_bias[0] +
+        (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens /
+               16384);
+    data[1] =
+        inv_obj.compass_bias[1] +
+        (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens /
+               16384);
+    data[2] =
+        inv_obj.compass_bias[2] +
+        (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens /
+               16384);
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data.
+ *          The argument array elements are ordered gyroscope X,Y, and Z,
+ *          accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
+ *          \if UMPL The magnetometer elements are not populated in UMPL. \endif
+ *          The gyroscope and accelerometer data is not scaled or offset, it is
+ *          copied directly from the sensor registers.
+ *          In the case of accelerometers with 8-bit output resolution, the data
+ *          is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
+ *          full scale range
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+ /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_raw_data is used to get Raw magnetometer data.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_raw_data(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.compass_sensor_data[0];
+    data[1] = inv_obj.compass_sensor_data[1];
+    data[2] = inv_obj.compass_sensor_data[2];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_magnetometer is used to get magnetometer data.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_magnetometer(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0];
+    data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1];
+    data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_pressure is used to get Pressure data.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to data to be passed back to the user.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_pressure(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.pressure;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_heading is used to get heading from Rotation Matrix.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to data to be passed back to the user.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+
+inv_error_t inv_get_heading(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    result = inv_get_rot_mat_float(rotMatrix);
+    if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
+        tmp =
+            (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
+                    90.0f);
+    } else {
+        tmp =
+            (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
+                    90.0f);
+    }
+    if (tmp < 0) {
+        tmp += 360.0f;
+    }
+    data[0] = (long)((360 - tmp) * 65536.0f);
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_cal_matrix is used to get the gyroscope
+ *          calibration matrix. The gyroscope calibration matrix defines the relationship
+ *          between the gyroscope sensor axes and the sensor fusion solution axes.
+ *          Calibration matrix data members will have a value of 1, 0, or -1.
+ *          The matrix has members
+ *          <center>C11 C12 C13</center>
+ *          <center>C21 C22 C23</center>
+ *          <center>C31 C32 C33</center>
+ *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_gyro_cal_matrix(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.gyro_cal[0];
+    data[1] = inv_obj.gyro_cal[1];
+    data[2] = inv_obj.gyro_cal[2];
+    data[3] = inv_obj.gyro_cal[3];
+    data[4] = inv_obj.gyro_cal[4];
+    data[5] = inv_obj.gyro_cal[5];
+    data[6] = inv_obj.gyro_cal[6];
+    data[7] = inv_obj.gyro_cal[7];
+    data[8] = inv_obj.gyro_cal[8];
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_accel_cal_matrix is used to get the accelerometer
+ *          calibration matrix.
+ *          Calibration matrix data members will have a value of 1, 0, or -1.
+ *          The matrix has members
+ *          <center>C11 C12 C13</center>
+ *          <center>C21 C22 C23</center>
+ *          <center>C31 C32 C33</center>
+ *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_accel_cal_matrix(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.accel_cal[0];
+    data[1] = inv_obj.accel_cal[1];
+    data[2] = inv_obj.accel_cal[2];
+    data[3] = inv_obj.accel_cal[3];
+    data[4] = inv_obj.accel_cal[4];
+    data[5] = inv_obj.accel_cal[5];
+    data[6] = inv_obj.accel_cal[6];
+    data[7] = inv_obj.accel_cal[7];
+    data[8] = inv_obj.accel_cal[8];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_cal_matrix is used to get magnetometer calibration matrix.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long at least</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_cal_matrix(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.compass_cal[0];
+    data[1] = inv_obj.compass_cal[1];
+    data[2] = inv_obj.compass_cal[2];
+    data[3] = inv_obj.compass_cal[3];
+    data[4] = inv_obj.compass_cal[4];
+    data[5] = inv_obj.compass_cal[5];
+    data[6] = inv_obj.compass_cal[6];
+    data[7] = inv_obj.compass_cal[7];
+    data[8] = inv_obj.compass_cal[8];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_bias_error is used to get magnetometer Bias error.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long at least</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_bias_error(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    if (inv_obj.large_field == 0) {
+        data[0] = inv_obj.compass_bias_error[0];
+        data[1] = inv_obj.compass_bias_error[1];
+        data[2] = inv_obj.compass_bias_error[2];
+    } else {
+        data[0] = P_INIT;
+        data[1] = P_INIT;
+        data[2] = P_INIT;
+    }
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_scale is used to get magnetometer scale.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long at least</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_scale(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.compass_scale[0];
+    data[1] = inv_obj.compass_scale[1];
+    data[2] = inv_obj.compass_scale[2];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_local_field is used to get local magnetic field data.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long at least</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_local_field(long *data)
+{
+    inv_error_t result = INV_SUCCESS;
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = inv_obj.local_field[0];
+    data[1] = inv_obj.local_field[1];
+    data[2] = inv_obj.local_field[2];
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_relative_quaternion is used to get relative quaternion.
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 4 cells long at least</b>.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ *  @endcond
+ */
+/* inv_get_relative_quaternion implemented in mlFIFO.c */
+
+/**
+ *  @brief  inv_get_gyro_float is used to get the most recent gyroscope measurement.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of dps (degrees per second).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gyro_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[3];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    result = inv_get_gyro(ldata);
+    data[0] = (float)ldata[0] / 65536.0f;
+    data[1] = (float)ldata[1] / 65536.0f;
+    data[2] = (float)ldata[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @internal
+ *  @brief  inv_get_angular_velocity_float is used to get an array of three data points representing the angular
+ *          velocity as derived from <b>both</b> gyroscopes and accelerometers.
+ *          This requires that ML_SENSOR_FUSION be enabled, to fuse data from
+ *          the gyroscope and accelerometer device, appropriately scaled and
+ *          oriented according to the respective mounting matrices.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_angular_velocity_float(float *data)
+{
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    /* not implemented. old (invalid) implementation:
+       return inv_get_gyro_float(data);
+     */
+}
+
+/**
+ *  @brief  inv_get_accel_float is used to get the most recent accelerometer measurement.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of g (gravity).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+ /* inv_get_accel_float implemented in mlFIFO.c */
+
+/**
+ *  @brief  inv_get_temperature_float is used to get the most recent
+ *          temperature measurement.
+ *          The argument array should only have one element.
+ *          The value is in units of deg C (degrees Celsius).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to data to be passed back to the user.
+ *
+ *  @return Zero if the command is successful; an ML error code otherwise.
+ */
+inv_error_t inv_get_temperature_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[1];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_temperature(ldata);
+    data[0] = (float)ldata[0] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_rot_mat_float is used to get an array of nine data points representing the rotation
+ *          matrix generated from all available sensors.
+ *          The array format will be R11, R12, R13, R21, R22, R23, R31, R32,
+ *          R33, representing the matrix:
+ *          <center>R11 R12 R13</center>
+ *          <center>R21 R22 R23</center>
+ *          <center>R31 R32 R33</center>
+ *          <b>Please refer to the "9-Axis Sensor Fusion Application Note" document,
+ *          section 7 "Sensor Fusion Output", for details regarding rotation
+ *          matrix output</b>.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long at least</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_rot_mat_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    {
+        long qdata[4], rdata[9];
+        inv_get_quaternion(qdata);
+        inv_quaternion_to_rotation(qdata, rdata);
+        data[0] = (float)rdata[0] / 1073741824.0f;
+        data[1] = (float)rdata[1] / 1073741824.0f;
+        data[2] = (float)rdata[2] / 1073741824.0f;
+        data[3] = (float)rdata[3] / 1073741824.0f;
+        data[4] = (float)rdata[4] / 1073741824.0f;
+        data[5] = (float)rdata[5] / 1073741824.0f;
+        data[6] = (float)rdata[6] / 1073741824.0f;
+        data[7] = (float)rdata[7] / 1073741824.0f;
+        data[8] = (float)rdata[8] / 1073741824.0f;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_quaternion_float is used to get the quaternion representation
+ *          of the current sensor fusion solution.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 4 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an ML error code otherwise.
+ */
+ /* inv_get_quaternion_float implemented in mlFIFO.c */
+
+/**
+ *  @brief  inv_get_linear_accel_float is used to get an estimate of linear
+ *          acceleration, based on the most recent accelerometer measurement
+ *          and sensor fusion solution.
+ *          The values are in units of g (gravity).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_linear_accel_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[3];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_linear_accel(ldata);
+    data[0] = (float)ldata[0] / 65536.0f;
+    data[1] = (float)ldata[1] / 65536.0f;
+    data[2] = (float)ldata[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_linear_accel_in_world_float is used to get an estimate of
+ *          linear acceleration, in the world frame,  based on the most
+ *          recent accelerometer measurement and sensor fusion solution.
+ *          The values are in units of g (gravity).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_linear_accel_in_world_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[3];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_linear_accel_in_world(ldata);
+    data[0] = (float)ldata[0] / 65536.0f;
+    data[1] = (float)ldata[1] / 65536.0f;
+    data[2] = (float)ldata[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gravity_float is used to get an estimate of the body frame
+ *          gravity vector, based on the most recent sensor fusion solution.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long at least</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gravity_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[3];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    result = inv_get_gravity(ldata);
+    data[0] = (float)ldata[0] / 65536.0f;
+    data[1] = (float)ldata[1] / 65536.0f;
+    data[2] = (float)ldata[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_cal_matrix_float is used to get the gyroscope
+ *          calibration matrix. The gyroscope calibration matrix defines the relationship
+ *          between the gyroscope sensor axes and the sensor fusion solution axes.
+ *          Calibration matrix data members will have a value of 1.0, 0, or -1.0.
+ *          The matrix has members
+ *          <center>C11 C12 C13</center>
+ *          <center>C21 C22 C23</center>
+ *          <center>C31 C32 C33</center>
+ *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gyro_cal_matrix_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f;
+    data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f;
+    data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f;
+    data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f;
+    data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f;
+    data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f;
+    data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f;
+    data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f;
+    data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_accel_cal_matrix_float is used to get the accelerometer
+ *          calibration matrix.
+ *          Calibration matrix data members will have a value of 1.0, 0, or -1.0.
+ *          The matrix has members
+ *          <center>C11 C12 C13</center>
+ *          <center>C21 C22 C23</center>
+ *          <center>C31 C32 C33</center>
+ *          The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+
+inv_error_t inv_get_accel_cal_matrix_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f;
+    data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f;
+    data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f;
+    data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f;
+    data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f;
+    data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f;
+    data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f;
+    data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f;
+    data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_cal_matrix_float is used to get an array of nine data points
+ *			representing the calibration matrix for the compass:
+ *          <center>C11 C12 C13</center>
+ *          <center>C21 C22 C23</center>
+ *          <center>C31 C32 C33</center>
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long at least</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_cal_matrix_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f;
+    data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f;
+    data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f;
+    data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f;
+    data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f;
+    data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f;
+    data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f;
+    data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f;
+    data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f;
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_temp_slope_float is used to get the temperature
+ *          compensation algorithm's estimate of the gyroscope bias temperature
+ *          coefficient.
+ *          The argument array elements are ordered X,Y,Z.
+ *          Values are in units of dps per deg C (degrees per second per degree
+ *          Celcius)
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long </b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gyro_temp_slope_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
+        data[0] = inv_obj.x_gyro_coef[1];
+        data[1] = inv_obj.y_gyro_coef[1];
+        data[2] = inv_obj.z_gyro_coef[1];
+    } else {
+        data[0] = (float)inv_obj.temp_slope[0] / 65536.0f;
+        data[1] = (float)inv_obj.temp_slope[1] / 65536.0f;
+        data[2] = (float)inv_obj.temp_slope[2] / 65536.0f;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_bias_float is used to get the gyroscope biases.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of dps (degrees per second).
+
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gyro_bias_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f;
+    data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f;
+    data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_accel_bias_float is used to get the accelerometer baises.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of g (gravity).
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_accel_bias_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.accel_bias[0] / 65536.0f;
+    data[1] = (float)inv_obj.accel_bias[1] / 65536.0f;
+    data[2] = (float)inv_obj.accel_bias[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_bias_float is used to get an array of three data points representing
+ *			the compass biases.
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_bias_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] =
+        ((float)
+         (inv_obj.compass_bias[0] +
+          (long)((long long)inv_obj.init_compass_bias[0] *
+                 inv_obj.compass_sens / 16384))) / 65536.0f;
+    data[1] =
+        ((float)
+         (inv_obj.compass_bias[1] +
+          (long)((long long)inv_obj.init_compass_bias[1] *
+                 inv_obj.compass_sens / 16384))) / 65536.0f;
+    data[2] =
+        ((float)
+         (inv_obj.compass_bias[2] +
+          (long)((long long)inv_obj.init_compass_bias[2] *
+                 inv_obj.compass_sens / 16384))) / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data.
+ *          The argument array elements are ordered gyroscope X,Y, and Z,
+ *          accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
+ *          \if UMPL The magnetometer elements are not populated in UMPL. \endif
+ *          The gyroscope and accelerometer data is not scaled or offset, it is
+ *          copied directly from the sensor registers, and cast as a float.
+ *          In the case of accelerometers with 8-bit output resolution, the data
+ *          is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
+ *          full scale range
+
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 9 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_gyro_and_accel_sensor_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    long ldata[6];
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_gyro_and_accel_sensor(ldata);
+    data[0] = (float)ldata[0];
+    data[1] = (float)ldata[1];
+    data[2] = (float)ldata[2];
+    data[3] = (float)ldata[3];
+    data[4] = (float)ldata[4];
+    data[5] = (float)ldata[5];
+    data[6] = (float)inv_obj.compass_sensor_data[0];
+    data[7] = (float)inv_obj.compass_sensor_data[1];
+    data[8] = (float)inv_obj.compass_sensor_data[2];
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_euler_angles_x is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          Euler angles are returned according to the X convention.
+ *          This is typically the convention used for mobile devices where the X
+ *          axis is the width of the screen, Y axis is the height, and Z the
+ *          depth. In this case roll is defined as the rotation around the X
+ *          axis of the device.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>Y axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
+ *
+           </TABLE>
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_x_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[6];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (float)(atan2f(rotMatrix[7],
+                       rotMatrix[8]) * 57.29577951308);
+    data[1] = (float)((double)asin(tmp) * 57.29577951308);
+    data[2] =
+        (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308);
+
+    return result;
+}
+
+/**
+ *  @brief  inv_get_euler_angles_float is used to get an array of three data points three data points
+ *			representing roll, pitch, and yaw corresponding to the INV_EULER_ANGLES_X output and it is
+ *          therefore the default convention for Euler angles.
+ *          Please refer to the INV_EULER_ANGLES_X for a detailed description.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_float(float *data)
+{
+    return inv_get_euler_angles_x_float(data);
+}
+
+/**  @brief  inv_get_euler_angles_y_float is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          Euler angles are returned according to the Y convention.
+ *          This convention is typically used in augmented reality applications,
+ *          where roll is defined as the rotation around the axis along the
+ *          height of the screen of a mobile device, namely the Y axis.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Y axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Z axis                </TD></TR>
+ *          </TABLE>
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_y_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[7];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308);
+    data[1] = (float)((double)asin(tmp) * 57.29577951308);
+    data[2] =
+        (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308);
+
+    return result;
+}
+
+/**  @brief  inv_get_euler_angles_z_float is used to get the Euler angle representation
+ *          of the current sensor fusion solution.
+ *          This convention is mostly used in application involving the use
+ *          of a camera, typically placed on the back of a mobile device, that
+ *          is along the Z axis.  In this convention roll is defined as the
+ *          rotation around the Z axis.
+ *          Euler angles are returned according to the Y convention.
+ *          <TABLE>
+ *          <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
+ *          <TR><TD> 0      </TD><TD>Roll              </TD><TD>Z axis                </TD></TR>
+ *          <TR><TD> 1      </TD><TD>Pitch             </TD><TD>X axis                </TD></TR>
+ *          <TR><TD> 2      </TD><TD>Yaw               </TD><TD>Y axis                </TD></TR>
+ *          </TABLE>
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_get_euler_angles_z_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    result = inv_get_rot_mat_float(rotMatrix);
+    tmp = rotMatrix[8];
+    if (tmp > 1.0f) {
+        tmp = 1.0f;
+    }
+    if (tmp < -1.0f) {
+        tmp = -1.0f;
+    }
+    data[0] =
+        (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308);
+    data[1] = (float)((double)asin(tmp) * 57.29577951308);
+    data[2] =
+        (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308);
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_raw_data_float is used to get Raw magnetometer data
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_raw_data_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] =
+        (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]);
+    data[1] =
+        (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]);
+    data[2] =
+        (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]);
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_magnetometer_float is used to get magnetometer data
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_magnetometer_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f;
+    data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f;
+    data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_pressure_float is used to get a single value representing the pressure in Pascal
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to the data to be passed back to the user.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_pressure_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.pressure;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_heading_float is used to get single number representing the heading of the device
+ *          relative to the Earth, in which 0 represents North, 90 degrees
+ *          represents East, and so on.
+ *          The heading is defined as the direction of the +Y axis if the Y
+ *          axis is horizontal, and otherwise the direction of the -Z axis.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to the data to be passed back to the user.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_heading_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    float rotMatrix[9];
+    float tmp;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    inv_get_rot_mat_float(rotMatrix);
+    if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
+        tmp =
+            (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
+                    90.0f);
+    } else {
+        tmp =
+            (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
+                    90.0f);
+    }
+    if (tmp < 0) {
+        tmp += 360.0f;
+    }
+    data[0] = 360 - tmp;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_bias_error_float is used to get an array of three numbers representing
+ *			the current estimated error in the compass biases. These numbers are unitless and serve
+ *          as rough estimates in which numbers less than 100 typically represent
+ *          reasonably well calibrated compass axes.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_bias_error_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (inv_obj.large_field == 0) {
+        data[0] = (float)inv_obj.compass_bias_error[0];
+        data[1] = (float)inv_obj.compass_bias_error[1];
+        data[2] = (float)inv_obj.compass_bias_error[2];
+    } else {
+        data[0] = (float)P_INIT;
+        data[1] = (float)P_INIT;
+        data[2] = (float)P_INIT;
+    }
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_mag_scale_float is used to get magnetometer scale.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_mag_scale_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.compass_scale[0] / 65536.0f;
+    data[1] = (float)inv_obj.compass_scale[1] / 65536.0f;
+    data[2] = (float)inv_obj.compass_scale[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_local_field_float is used to get local magnetic field data.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 3 cells long</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_local_field_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.local_field[0] / 65536.0f;
+    data[1] = (float)inv_obj.local_field[1] / 65536.0f;
+    data[2] = (float)inv_obj.local_field[2] / 65536.0f;
+
+    return result;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_get_relative_quaternion_float is used to get relative quaternion data.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
+ *          must have been called.
+ *
+ *  @param  data
+ *              A pointer to an array to be passed back to the user.
+ *              <b>Must be 4 cells long at least</b>.
+ *
+ *  @return INV_SUCCESS if the command is successful; an error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_get_relative_quaternion_float(float *data)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+
+    if (inv_get_state() < INV_STATE_DMP_OPENED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f;
+    data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f;
+    data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f;
+    data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f;
+
+    return result;
+}
+
+/**
+ * Returns the curren compass accuracy.
+ *
+ * - 0: Unknown: The accuracy is unreliable and compass data should not be used
+ * - 1: Low: The compass accuracy is low.
+ * - 2: Medium: The compass accuracy is medium.
+ * - 3: High: The compas acurracy is high and can be trusted
+ *
+ * @param accuracy The accuracy level in the range 0-3
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+inv_error_t inv_get_compass_accuracy(int *accuracy)
+{
+    if (inv_get_state() != INV_STATE_DMP_STARTED)
+        return INV_ERROR_SM_IMPROPER_STATE;
+
+    *accuracy = inv_obj.compass_accuracy;
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  inv_set_gyro_bias is used to set the gyroscope bias.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are scaled at 1 dps = 2^16 LSBs.
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_gyro_bias(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+    long biasTmp;
+    long sf = 0;
+    short offset[GYRO_NUM_AXES];
+    int i;
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    if (mldl_cfg->gyro_sens_trim != 0) {
+        sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
+    } else {
+        sf = 2000;
+    }
+    for (i = 0; i < GYRO_NUM_AXES; i++) {
+        inv_obj.gyro_bias[i] = data[i];
+        biasTmp = -inv_obj.gyro_bias[i] / sf;
+        if (biasTmp < 0)
+            biasTmp += 65536L;
+        offset[i] = (short)biasTmp;
+    }
+    result = inv_set_offset(offset);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  inv_set_accel_bias is used to set the accelerometer bias.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are scaled in units of g (gravity),
+ *          where 1 g = 2^16 LSBs.
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_accel_bias(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+    long biasTmp;
+    int i, j;
+    unsigned char regs[6];
+    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+    for (i = 0; i < ACCEL_NUM_AXES; i++) {
+        inv_obj.accel_bias[i] = data[i];
+        if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) {
+            long long tmp64;
+            inv_obj.scaled_accel_bias[i] = 0;
+            for (j = 0; j < ACCEL_NUM_AXES; j++) {
+                inv_obj.scaled_accel_bias[i] +=
+                    data[j] *
+                    (long)mldl_cfg->pdata->accel.orientation[i * 3 + j];
+            }
+            tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13;
+            biasTmp = (long)(tmp64 / inv_obj.accel_sens);
+        } else {
+            biasTmp = 0;
+        }
+        if (biasTmp < 0)
+            biasTmp += 65536L;
+        regs[2 * i + 0] = (unsigned char)(biasTmp / 256);
+        regs[2 * i + 1] = (unsigned char)(biasTmp % 256);
+    }
+    result = inv_set_mpu_memory(KEY_D_1_8, 2, &regs[0]);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_mpu_memory(KEY_D_1_10, 2, &regs[2]);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_set_mpu_memory(KEY_D_1_2, 2, &regs[4]);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_mag_bias is used to set Compass Bias
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_mag_bias(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    inv_set_compass_bias(data);
+    inv_obj.init_compass_bias[0] = 0;
+    inv_obj.init_compass_bias[1] = 0;
+    inv_obj.init_compass_bias[2] = 0;
+    inv_obj.got_compass_bias = 1;
+    inv_obj.got_init_compass_bias = 1;
+    inv_obj.compass_state = SF_STARTUP_SETTLE;
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  inv_set_gyro_temp_slope is used to set the temperature
+ *          compensation algorithm's estimate of the gyroscope bias temperature
+ *          coefficient.
+ *          The argument array elements are ordered X,Y,Z.
+ *          Values are in units of dps per deg C (degrees per second per degree
+ *          Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs.
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document.
+ *
+ *  @brief  inv_set_gyro_temp_slope is used to set Gyro temperature slope
+ *
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_gyro_temp_slope(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+    int i;
+    long sf;
+    unsigned char regs[3];
+
+    inv_obj.factory_temp_comp = 1;
+    inv_obj.temp_slope[0] = data[0];
+    inv_obj.temp_slope[1] = data[1];
+    inv_obj.temp_slope[2] = data[2];
+    for (i = 0; i < GYRO_NUM_AXES; i++) {
+        sf = -inv_obj.temp_slope[i] / 1118;
+        if (sf > 127) {
+            sf -= 256;
+        }
+        regs[i] = (unsigned char)sf;
+    }
+    result = inv_set_offsetTC(regs);
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_local_field is used to set local magnetic field
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_local_field(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    inv_obj.local_field[0] = data[0];
+    inv_obj.local_field[1] = data[1];
+    inv_obj.local_field[2] = data[2];
+    inv_obj.new_local_field = 1;
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_mag_scale is used to set magnetometer scale
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_mag_scale(long *data)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    inv_obj.compass_scale[0] = data[0];
+    inv_obj.compass_scale[1] = data[1];
+    inv_obj.compass_scale[2] = data[2];
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  inv_set_gyro_temp_slope_float is used to get the temperature
+ *          compensation algorithm's estimate of the gyroscope bias temperature
+ *          coefficient.
+ *          The argument array elements are ordered X,Y,Z.
+ *          Values are in units of dps per deg C (degrees per second per degree
+ *          Celcius)
+
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_gyro_temp_slope_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_gyro_temp_slope(arrayTmp);
+}
+
+/**
+ *  @brief  inv_set_gyro_bias_float is used to set the gyroscope bias.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of dps (degrees per second).
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_gyro_bias_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_gyro_bias(arrayTmp);
+
+}
+
+/**
+ *  @brief  inv_set_accel_bias_float is used to set the accelerometer bias.
+ *          The argument array elements are ordered X,Y,Z.
+ *          The values are in units of g (gravity).
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ */
+inv_error_t inv_set_accel_bias_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_accel_bias(arrayTmp);
+
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_mag_bias_float is used to set compass bias
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen()\ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_mag_bias_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_mag_bias(arrayTmp);
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_local_field_float is used to set local magnetic field
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_local_field_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_local_field(arrayTmp);
+}
+
+/**
+ *  @cond MPL
+ *  @brief  inv_set_mag_scale_float is used to set magnetometer scale
+ *
+ *          Please refer to the provided "9-Axis Sensor Fusion
+ *          Application Note" document provided.
+ *
+ *  @pre    MLDmpOpen() \ifnot UMPL or
+ *          MLDmpPedometerStandAloneOpen() \endif
+ *  @pre    MLDmpStart() must <b>NOT</b> have been called.
+ *
+ *  @param  data        A pointer to an array to be copied from the user.
+ *
+ *  @return INV_SUCCESS if successful; a non-zero error code otherwise.
+ *  @endcond
+ */
+inv_error_t inv_set_mag_scale_float(float *data)
+{
+    long arrayTmp[3];
+    arrayTmp[0] = (long)(data[0] * 65536.f);
+    arrayTmp[1] = (long)(data[1] * 65536.f);
+    arrayTmp[2] = (long)(data[2] * 65536.f);
+    return inv_set_mag_scale(arrayTmp);
+}
diff --git a/60xx/mlsdk/mllite/mlarray_legacy.c b/60xx/mlsdk/mllite/mlarray_legacy.c
new file mode 100644
index 0000000..9dce8f3
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlcontrol.c b/60xx/mlsdk/mllite/mlcontrol.c
new file mode 100644
index 0000000..bd186fa
--- /dev/null
+++ b/60xx/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)
+{
+    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/60xx/mlsdk/mllite/mlcontrol.h b/60xx/mlsdk/mllite/mlcontrol.h
new file mode 100644
index 0000000..a834fc6
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mldl.c b/60xx/mlsdk/mllite/mldl.c
new file mode 100644
index 0000000..7054532
--- /dev/null
+++ b/60xx/mlsdk/mllite/mldl.c
@@ -0,0 +1,1092 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+/******************************************************************************
+ *
+ * $Id: mldl.c 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *****************************************************************************/
+
+/**
+ *  @defgroup   MLDL 
+ *  @brief      Motion Library - Driver Layer.
+ *              The Motion Library Driver Layer provides the intrface to the 
+ *              system drivers that are used by the Motion Library.
+ *
+ *  @{
+ *      @file   mldl.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+
+#include "mpu.h"
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+#    include "mpu6050a2.h"
+#elif defined CONFIG_MPU_SENSORS_MPU6050B1
+#    include "mpu6050b1.h"
+#elif defined CONFIG_MPU_SENSORS_MPU3050
+#  include "mpu3050.h"
+#else
+#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
+#endif
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "compass.h"
+#include "mlsl.h"
+#include "mlos.h"
+#include "mlinclude.h"
+#include "ml.h"
+#include "dmpKey.h"
+#include "mlFIFOHW.h"
+#include "compass.h"
+#include "pressure.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mldl"
+
+#define _mldlDebug(x)           //{x}
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2)   /* 128 */
+
+/*---- structure containing control variables used by MLDL ----*/
+static struct mldl_cfg mldlCfg;
+struct ext_slave_descr gAccel;
+struct ext_slave_descr gCompass;
+struct ext_slave_descr gPressure;
+struct mpu_platform_data gPdata;
+static void *sMLSLHandle;
+int_fast8_t intTrigger[NUM_OF_INTSOURCES];
+
+/*******************************************************************************
+ * Functions for accessing the DMP memory via keys
+ ******************************************************************************/
+
+unsigned short (*sGetAddress) (unsigned short key) = NULL;
+static const unsigned char *localDmpMemory = NULL;
+static unsigned short localDmpMemorySize = 0;
+
+/**
+ *  @internal
+ *  @brief Sets the function to use to convert keys to addresses. This
+ *         will changed for each DMP code loaded.
+ *  @param func
+ *              Function used to convert keys to addresses.
+ *  @endif
+ */
+void inv_set_get_address(unsigned short (*func) (unsigned short key))
+{
+    INVENSENSE_FUNC_START;
+    _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func);
+        )
+        sGetAddress = func;
+}
+
+/**
+ *  @internal
+ *  @brief  Check if the feature is supported in the currently loaded
+ *          DMP code basing on the fact that the key is assigned a
+ *          value or not.
+ *  @param  key     the DMP key
+ *  @return whether the feature associated with the key is supported
+ *          or not.
+ */
+uint_fast8_t inv_dmpkey_supported(unsigned short key)
+{
+    unsigned short memAddr;
+
+    if (sGetAddress == NULL) {
+        MPL_LOGE("%s : sGetAddress is NULL\n", __func__);
+        return FALSE;
+    }
+
+    memAddr = sGetAddress(key);
+    if (memAddr >= 0xffff) {
+        MPL_LOGV("inv_set_mpu_memory unsupported key\n");
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/**
+ *  @internal
+ *  @brief  used to get the specified number of bytes from the original
+ *          MPU memory location specified by the key.
+ *          Reads the specified number of bytes from the MPU location
+ *          that was used to program the MPU specified by the key. Each
+ *          set of code specifies a function that changes keys into
+ *          addresses. This function is set with setGetAddress().
+ *
+ *  @param  key     The key to use when looking up the address.
+ *  @param  length  Number of bytes to read.
+ *  @param  buffer  Result for data.
+ *
+ *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
+ *          not corresponding to a memory address will result in INV_ERROR.
+ *  @endif
+ */
+inv_error_t inv_get_mpu_memory_original(unsigned short key,
+                                        unsigned short length,
+                                        unsigned char *buffer)
+{
+    unsigned short offset;
+
+    if (sGetAddress == NULL) {
+        return INV_ERROR_NOT_OPENED;
+    }
+
+    offset = sGetAddress(key);
+    if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    memcpy(buffer, &localDmpMemory[offset], length);
+
+    return INV_SUCCESS;
+}
+
+unsigned short inv_dl_get_address(unsigned short key)
+{
+    unsigned short offset;
+    if (sGetAddress == NULL) {
+        return INV_ERROR_NOT_OPENED;
+    }
+
+    offset = sGetAddress(key);
+    return offset;
+}
+
+/* ---------------------- */
+/* -  Static Functions. - */
+/* ---------------------- */
+
+/**
+ *  @brief  Open the driver layer and resets the internal
+ *          gyroscope, accelerometer, and compass data
+ *          structures.
+ *  @param  mlslHandle
+ *              the serial handle.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_dl_open(void *mlslHandle)
+{
+    inv_error_t result;
+    memset(&mldlCfg, 0, sizeof(mldlCfg));
+    memset(intTrigger, INT_CLEAR, sizeof(intTrigger));
+
+    sMLSLHandle = mlslHandle;
+
+    mldlCfg.addr  = 0x68; /* default incase the driver doesn't set it */
+    mldlCfg.accel = &gAccel;
+    mldlCfg.compass = &gCompass;
+    mldlCfg.pressure = &gPressure;
+    mldlCfg.pdata = &gPdata;
+
+    result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle,
+                                        sMLSLHandle, sMLSLHandle, sMLSLHandle);
+    return result;
+}
+
+/**
+ *  @brief  Closes/Cleans up the ML Driver Layer.
+ *          Put the device in sleep mode.
+ *  @return INV_SUCCESS or non-zero error code.
+ */
+inv_error_t inv_dl_close(void)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    result = (inv_error_t) inv_mpu_suspend(&mldlCfg,
+                                           sMLSLHandle,
+                                           sMLSLHandle,
+                                           sMLSLHandle,
+                                           sMLSLHandle, 
+                                           INV_ALL_SENSORS);
+
+    result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle,
+                                         sMLSLHandle, sMLSLHandle, sMLSLHandle);
+    /* Clear all previous settings */
+    memset(&mldlCfg, 0, sizeof(mldlCfg));
+    sMLSLHandle = NULL;
+    sGetAddress = NULL;
+    return result;
+}
+
+/**
+ * @brief Sets the requested_sensors
+ *
+ * Accessor to set the requested_sensors field of the mldl_cfg structure.
+ * Typically set at initialization.
+ *
+ * @param sensors
+ * Bitfield of the sensors that are going to be used.  Combination of the
+ * following:
+ *  - INV_X_GYRO
+ *  - INV_Y_GYRO
+ *  - INV_Z_GYRO
+ *  - INV_DMP_PROCESSOR
+ *  - INV_X_ACCEL
+ *  - INV_Y_ACCEL
+ *  - INV_Z_ACCEL
+ *  - INV_X_COMPASS
+ *  - INV_Y_COMPASS
+ *  - INV_Z_COMPASS
+ *  - INV_X_PRESSURE
+ *  - INV_Y_PRESSURE
+ *  - INV_Z_PRESSURE
+ *  - INV_THREE_AXIS_GYRO
+ *  - INV_THREE_AXIS_ACCEL
+ *  - INV_THREE_AXIS_COMPASS
+ *  - INV_THREE_AXIS_PRESSURE
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t inv_init_requested_sensors(unsigned long sensors)
+{
+    mldlCfg.requested_sensors = sensors;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief Starts the DMP running
+ *
+ * Resumes the sensor if any of the sensor axis or components are requested
+ *
+ * @param sensors
+ * Bitfield of the sensors to turn on.  Combination of the following:
+ *  - INV_X_GYRO
+ *  - INV_Y_GYRO
+ *  - INV_Z_GYRO
+ *  - INV_DMP_PROCESSOR
+ *  - INV_X_ACCEL
+ *  - INV_Y_ACCEL
+ *  - INV_Z_ACCEL
+ *  - INV_X_COMPASS
+ *  - INV_Y_COMPASS
+ *  - INV_Z_COMPASS
+ *  - INV_X_PRESSURE
+ *  - INV_Y_PRESSURE
+ *  - INV_Z_PRESSURE
+ *  - INV_THREE_AXIS_GYRO
+ *  - INV_THREE_AXIS_ACCEL
+ *  - INV_THREE_AXIS_COMPASS
+ *  - INV_THREE_AXIS_PRESSURE
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t inv_dl_start(unsigned long sensors)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    mldlCfg.requested_sensors = sensors;
+    result = inv_mpu_resume(&mldlCfg,
+                            sMLSLHandle,
+                            sMLSLHandle,
+                            sMLSLHandle,
+                            sMLSLHandle,
+                            sensors);
+    return result;
+}
+
+/**
+ * @brief Stops the DMP running and puts it in low power as requested
+ *
+ * Suspends each sensor according to the bitfield, if all axis and components
+ * of the sensor is off.
+ *
+ * @param sensors Bitfiled of the sensors to leave on.  Combination of the
+ * following:
+ *  - INV_X_GYRO
+ *  - INV_Y_GYRO
+ *  - INV_Z_GYRO
+ *  - INV_X_ACCEL
+ *  - INV_Y_ACCEL
+ *  - INV_Z_ACCEL
+ *  - INV_X_COMPASS
+ *  - INV_Y_COMPASS
+ *  - INV_Z_COMPASS
+ *  - INV_X_PRESSURE
+ *  - INV_Y_PRESSURE
+ *  - INV_Z_PRESSURE
+ *  - INV_THREE_AXIS_GYRO
+ *  - INV_THREE_AXIS_ACCEL
+ *  - INV_THREE_AXIS_COMPASS
+ *  - INV_THREE_AXIS_PRESSURE
+ *
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t inv_dl_stop(unsigned long sensors)
+{
+    INVENSENSE_FUNC_START;
+    inv_error_t result = INV_SUCCESS;
+
+    result = inv_mpu_suspend(&mldlCfg,
+                             sMLSLHandle,
+                             sMLSLHandle,
+                             sMLSLHandle,
+                             sMLSLHandle,
+                             sensors);
+    return result;
+}
+
+/**
+ *  @brief  Get a pointer to the internal data structure
+ *          storing the configuration for the MPU, the accelerometer
+ *          and the compass in use.
+ *  @return a pointer to the data structure of type 'struct mldl_cfg'.
+ */
+struct mldl_cfg *inv_get_dl_config(void)
+{
+    return &mldlCfg;
+}
+
+/**
+ *  @brief   Query the MPU slave address.
+ *  @return  The 7-bit mpu slave address.
+ */
+unsigned char inv_get_mpu_slave_addr(void)
+{
+    INVENSENSE_FUNC_START;
+    return mldlCfg.addr;
+}
+
+/**
+ *  @internal
+ * @brief   MLDLCfgDMP configures the Digital Motion Processor internal to
+ *          the MPU. The DMP can be enabled or disabled and the start address
+ *          can be set.
+ *
+ * @param   enableRun   Enables the DMP processing if set to TRUE.
+ * @param   enableFIFO  Enables DMP output to the FIFO if set to TRUE.
+ * @param   startAddress start address
+ *
+ * @return  Zero if the command is successful, an error code otherwise.
+*/
+inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
+                                unsigned char enableFIFO)
+{
+    INVENSENSE_FUNC_START;
+
+    mldlCfg.dmp_enable = enableRun;
+    mldlCfg.fifo_enable = enableFIFO;
+    mldlCfg.gyro_needs_reset = TRUE;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   inv_get_dl_cfg_int configures the interrupt function on the specified pin.
+ *          The basic interrupt signal characteristics can be set
+ *          (i.e. active high/low, open drain/push pull, etc.) and the
+ *          triggers can be set.
+ *          Currently only INTPIN_MPU is supported.
+ *
+ * @param   triggers
+ *              bitmask of triggers to enable for interrupt.
+ *              The available triggers are:
+ *              - BIT_MPU_RDY_EN
+ *              - BIT_DMP_INT_EN
+ *              - BIT_RAW_RDY_EN
+ *
+ * @return  Zero if the command is successful, an error code otherwise.
+*/
+inv_error_t inv_get_dl_cfg_int(unsigned char triggers)
+{
+    inv_error_t result = INV_SUCCESS;
+
+#if defined CONFIG_MPU_SENSORS_MPU3050
+    /* Mantis has 8 bits of interrupt config bits */
+    if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+#endif
+
+    mldlCfg.int_config = triggers;
+    if (!mldlCfg.gyro_is_suspended) {
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_INT_CFG,
+                                         (mldlCfg.int_config | mldlCfg.pdata->
+                                          int_config));
+    } else {
+        mldlCfg.gyro_needs_reset = TRUE;
+    }
+
+    return result;
+}
+
+/**
+ * @brief   configures the output sampling rate on the MPU.
+ *          Three parameters control the sampling:
+ *
+ *          1) Low pass filter bandwidth, and
+ *          2) output sampling divider.
+ *
+ *          The output sampling rate is determined by the divider and the low
+ *          pass filter setting. If the low pass filter is set to
+ *          'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the
+ *          divider is 8kHz; for all other settings it is 1kHz.
+ *          The 8-bit divider will divide this frequency to get the resulting
+ *          sample frequency.
+ *          For example, if the filter setting is not 256Hz and the divider is
+ *          set to 7, then the sample rate is as follows:
+ *          sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms).
+ *
+ *          The low pass filter selection codes control both the cutoff frequency of
+ *          the internal low pass filter and internal analog sampling rate. The
+ *          latter, in turn, affects the final output sampling rate according to the
+ *          sample rate divider settig.
+ *              0 -> 256 Hz  cutoff BW, 8 kHz analog sample rate,
+ *              1 -> 188 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              2 ->  98 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              3 ->  42 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              4 ->  20 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              5 ->  10 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              6 ->   5 Hz  cutoff BW, 1 kHz analog sample rate,
+ *              7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate.
+ *
+ * @param   lpf         low pass filter,   0 to 7.
+ * @param   divider     Output sampling rate divider, 0 to 255.
+ *
+ * @return  ML_SUCESS if successful; a non-zero error code otherwise.
+**/
+inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider)
+{
+    /*---- do range checking ----*/
+    if (lpf >= NUM_MPU_FILTER) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    mldlCfg.lpf = lpf;
+    mldlCfg.divider = divider;
+    mldlCfg.gyro_needs_reset = TRUE;
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  set the full scale range for the gyros.
+ *          The full scale selection codes correspond to:
+ *              0 -> 250  dps,
+ *              1 -> 500  dps,
+ *              2 -> 1000 dps,
+ *              3 -> 2000 dps.
+ *          Full scale range affect the MPU's measurement
+ *          sensitivity.
+ *
+ *  @param  fullScale
+ *              the gyro full scale range in dps.
+ *
+ *  @return INV_SUCCESS or non-zero error code.
+**/
+inv_error_t inv_set_full_scale(float fullScale)
+{
+    if (fullScale == 250.f)
+        mldlCfg.full_scale = MPU_FS_250DPS;
+    else if (fullScale == 500.f)
+        mldlCfg.full_scale = MPU_FS_500DPS;
+    else if (fullScale == 1000.f)
+        mldlCfg.full_scale = MPU_FS_1000DPS;
+    else if (fullScale == 2000.f)
+        mldlCfg.full_scale = MPU_FS_2000DPS;
+    else {                      // not a valid setting
+        MPL_LOGE("Invalid full scale range specification for gyros : %f\n",
+                 fullScale);
+        MPL_LOGE
+            ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n");
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    mldlCfg.gyro_needs_reset = TRUE;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   This function sets the external sync for the MPU sampling.
+ *          It can be synchronized on the LSB of any of the gyros, any of the
+ *          external accels, or on the temp readings.
+ *
+ * @param   extSync External sync selection, 0 to 7.
+ * @return  Zero if the command is successful; an error code otherwise.
+**/
+inv_error_t inv_set_external_sync(unsigned char extSync)
+{
+    INVENSENSE_FUNC_START;
+
+    /*---- do range checking ----*/
+    if (extSync >= NUM_MPU_EXT_SYNC) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    mldlCfg.ext_sync = extSync;
+    mldlCfg.gyro_needs_reset = TRUE;
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_set_ignore_system_suspend(unsigned char ignore)
+{
+    INVENSENSE_FUNC_START;
+
+    mldlCfg.ignore_system_suspend = ignore;
+
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief   inv_clock_source function sets the clock source for the MPU gyro
+ *          processing.
+ *          The source can be any of the following:
+ *          - Internal 8MHz oscillator,
+ *          - PLL with X gyro as reference,
+ *          - PLL with Y gyro as reference,
+ *          - PLL with Z gyro as reference,
+ *          - PLL with external 32.768Mhz reference, or
+ *          - PLL with external 19.2MHz reference
+ *
+ *          For best accuracy and timing, it is highly recommended to use one
+ *          of the gyros as the clock source; however this gyro must be
+ *          enabled to use its clock (see 'MLDLPowerMgmtMPU()').
+ *
+ * @param   clkSource   Clock source selection.
+ *                      Can be one of:
+ *                      - CLK_INTERNAL,
+ *                      - CLK_PLLGYROX,
+ *                      - CLK_PLLGYROY,
+ *                      - CLK_PLLGYROZ,
+ *                      - CLK_PLLEXT32K, or
+ *                      - CLK_PLLEXT19M.
+ *
+ * @return  Zero if the command is successful; an error code otherwise.
+**/
+inv_error_t inv_clock_source(unsigned char clkSource)
+{
+    INVENSENSE_FUNC_START;
+
+    /*---- do range checking ----*/
+    if (clkSource >= NUM_CLK_SEL) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    mldlCfg.clk_src = clkSource;
+    mldlCfg.gyro_needs_reset = TRUE;
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Set the Temperature Compensation offset.
+ *  @param  tc
+ *              a pointer to the temperature compensations offset
+ *              for the 3 gyro axes.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_set_offsetTC(const unsigned char *tc)
+{
+    int ii;
+    inv_error_t result;
+
+    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) {
+        mldlCfg.offset_tc[ii] = tc[ii];
+    }
+
+    if (!mldlCfg.gyro_is_suspended) {
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_XG_OFFS_TC, tc[0]);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_YG_OFFS_TC, tc[1]);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_ZG_OFFS_TC, tc[2]);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+#else
+        unsigned char reg;
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_XG_OFFS_TC,
+                                         ((mldlCfg.offset_tc[0] << 1)
+                                          & BITS_XG_OFFS_TC));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC);
+#ifdef CONFIG_MPU_SENSORS_MPU6050B1
+        if (mldlCfg.pdata->level_shifter)
+            reg |= BIT_I2C_MST_VDDIO;
+#endif
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_YG_OFFS_TC, reg);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
+                                         MPUREG_ZG_OFFS_TC,
+                                         ((mldlCfg.offset_tc[2] << 1)
+                                          & BITS_ZG_OFFS_TC));
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+#endif
+    } else {
+        mldlCfg.gyro_needs_reset = TRUE;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Set the gyro offset.
+ *  @param  offset
+ *              a pointer to the gyro offset for the 3 gyro axes. This is scaled
+ *              as it would be written to the hardware registers.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_set_offset(const short *offset)
+{
+    inv_error_t result;
+    unsigned char regs[7];
+    int ii;
+    long sf;
+
+    sf = (2000L * 131) / mldlCfg.gyro_sens_trim;
+    for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) {
+        // Record the bias in the units the register uses
+        mldlCfg.offset[ii] = offset[ii];
+        // Convert the bias to 1 dps = 1<<16
+        inv_obj.gyro_bias[ii] = -offset[ii] * sf;
+        regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff;
+        regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff);
+    }
+
+    if (!mldlCfg.gyro_is_suspended) {
+        regs[0] = MPUREG_X_OFFS_USRH;
+        result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    } else {
+        mldlCfg.gyro_needs_reset = TRUE;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @internal
+ *  @brief  used to get the specified number of bytes in the specified MPU
+ *          memory bank.
+ *          The memory bank is one of the following:
+ *          - MPUMEM_RAM_BANK_0,
+ *          - MPUMEM_RAM_BANK_1,
+ *          - MPUMEM_RAM_BANK_2, or
+ *          - MPUMEM_RAM_BANK_3.
+ *
+ *  @param  bank    Memory bank to write.
+ *  @param  memAddr Starting address for write.
+ *  @param  length  Number of bytes to write.
+ *  @param  buffer  Result for data.
+ *
+ *  @return zero if the command is successful, an error code otherwise.
+ *  @endif
+ */
+inv_error_t
+inv_get_mpu_memory_one_bank(unsigned char bank,
+                            unsigned char memAddr,
+                            unsigned short length, unsigned char *buffer)
+{
+    inv_error_t result;
+
+    if ((bank >= MPU_MEM_NUM_RAM_BANKS) ||
+        //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255
+        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (mldlCfg.gyro_is_suspended) {
+        memcpy(buffer, &mldlCfg.ram[bank][memAddr], length);
+        result = INV_SUCCESS;
+    } else {
+        result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr,
+                                     ((bank << 8) | memAddr), length, buffer);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    return result;
+}
+
+/**
+ *  @internal
+ *  @brief  used to set the specified number of bytes in the specified MPU
+ *          memory bank.
+ *          The memory bank is one of the following:
+ *          - MPUMEM_RAM_BANK_0,
+ *          - MPUMEM_RAM_BANK_1,
+ *          - MPUMEM_RAM_BANK_2, or
+ *          - MPUMEM_RAM_BANK_3.
+ *
+ *  @param  bank    Memory bank to write.
+ *  @param  memAddr Starting address for write.
+ *  @param  length  Number of bytes to write.
+ *  @param  buffer  Result for data.
+ *
+ *  @return zero if the command is successful, an error code otherwise.
+ *  @endif
+ */
+inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank,
+                                        unsigned short memAddr,
+                                        unsigned short length,
+                                        const unsigned char *buffer)
+{
+    inv_error_t result = INV_SUCCESS;
+    int different;
+
+    if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) ||
+        ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length);
+    memcpy(&mldlCfg.ram[bank][memAddr], buffer, length);
+    if (!mldlCfg.gyro_is_suspended) {
+        result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr,
+                                      ((bank << 8) | memAddr), length, buffer);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    } else if (different) {
+        mldlCfg.gyro_needs_reset = TRUE;
+    }
+
+    return result;
+}
+
+/**
+ *  @internal
+ *  @brief  used to get the specified number of bytes from the MPU location
+ *          specified by the key.
+ *          Reads the specified number of bytes from the MPU location
+ *          specified by the key. Each set of code specifies a function
+ *          that changes keys into addresses. This function is set with
+ *          setGetAddress().
+ *
+ *  @param  key     The key to use when looking up the address.
+ *  @param  length  Number of bytes to read.
+ *  @param  buffer  Result for data.
+ *
+ *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
+ *          not corresponding to a memory address will result in INV_ERROR.
+ *  @endif
+ */
+inv_error_t inv_get_mpu_memory(unsigned short key,
+                               unsigned short length, unsigned char *buffer)
+{
+    unsigned char bank;
+    inv_error_t result;
+    unsigned short memAddr;
+
+    if (sGetAddress == NULL) {
+        return INV_ERROR_NOT_OPENED;
+    }
+
+    memAddr = sGetAddress(key);
+    if (memAddr >= 0xffff)
+        return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+    bank = memAddr >> 8;        // Get Bank
+    memAddr &= 0xff;
+
+    while (memAddr + length > MPU_MEM_BANK_SIZE) {
+        // We cross a bank in the middle
+        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
+        result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
+                                             sub_length, buffer);
+        if (INV_SUCCESS != result)
+            return result;
+        bank++;
+        length -= sub_length;
+        buffer += sub_length;
+        memAddr = 0;
+    }
+    result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
+                                         length, buffer);
+
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    return result;
+}
+
+/**
+ *  @internal
+ *  @brief  used to set the specified number of bytes from the MPU location
+ *          specified by the key.
+ *          Set the specified number of bytes from the MPU location
+ *          specified by the key. Each set of DMP code specifies a function
+ *          that changes keys into addresses. This function is set with
+ *          setGetAddress().
+ *
+ *  @param  key     The key to use when looking up the address.
+ *  @param  length  Number of bytes to write.
+ *  @param  buffer  Result for data.
+ *
+ *  @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
+ *          not corresponding to a memory address will result in INV_ERROR.
+ *  @endif
+ */
+inv_error_t inv_set_mpu_memory(unsigned short key,
+                               unsigned short length,
+                               const unsigned char *buffer)
+{
+    inv_error_t result = INV_SUCCESS;
+    unsigned short memAddr;
+    unsigned char bank;
+
+    if (sGetAddress == NULL) {
+        MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n");
+        return INV_ERROR_INVALID_MODULE;
+    }
+    memAddr = sGetAddress(key);
+
+    if (memAddr >= 0xffff) {
+        MPL_LOGE("inv_set_mpu_memory unsupported key\n");
+        return INV_ERROR_INVALID_MODULE; // This key not supported
+    }
+
+    bank = (unsigned char)(memAddr >> 8);
+    memAddr &= 0xff;
+
+    while (memAddr + length > MPU_MEM_BANK_SIZE) {
+        // We cross a bank in the middle
+        unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
+
+        result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        bank++;
+        length -= sub_length;
+        buffer += sub_length;
+        memAddr = 0;
+    }
+    result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+/**
+ *  @brief  Load the DMP with the given code and configuration.
+ *  @param  buffer
+ *              the DMP data.
+ *  @param  length
+ *              the length in bytes of the DMP data.
+ *  @param  config
+ *              the DMP configuration.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+inv_error_t inv_load_dmp(const unsigned char *buffer,
+                         unsigned short length, unsigned short config)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result = INV_SUCCESS;
+    unsigned short toWrite;
+    unsigned short memAddr = 0;
+    localDmpMemory = buffer;
+    localDmpMemorySize = length;
+
+    mldlCfg.dmp_cfg1 = (config >> 8);
+    mldlCfg.dmp_cfg2 = (config & 0xff);
+
+    while (length > 0) {
+        toWrite = length;
+        if (toWrite > MAX_LOAD_WRITE_SIZE)
+            toWrite = MAX_LOAD_WRITE_SIZE;
+
+        result =
+            inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite,
+                                        buffer);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        buffer += toWrite;
+        memAddr += toWrite;
+        length -= toWrite;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  Get the silicon revision ID.
+ *  @return The silicon revision ID
+ *          (0 will be read if inv_mpu_open returned an error)
+ */
+unsigned char inv_get_silicon_rev(void)
+{
+    return mldlCfg.silicon_revision;
+}
+
+/**
+ *  @brief  Get the product revision ID.
+ *  @return The product revision ID
+ *          (0 will be read if inv_mpu_open returned an error)
+ */
+unsigned char inv_get_product_rev(void)
+{
+    return mldlCfg.product_revision;
+}
+
+/*******************************************************************************
+ *******************************************************************************
+ *******************************************************************************
+ * @todo these belong with an interface to the kernel driver layer
+ *******************************************************************************
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * @brief   inv_get_interrupt_status returns the interrupt status from the specified
+ *          interrupt pin.
+ * @param   intPin
+ *              Currently only the value INTPIN_MPU is supported.
+ * @param   status
+ *              The available statuses are:
+ *              - BIT_MPU_RDY_EN
+ *              - BIT_DMP_INT_EN
+ *              - BIT_RAW_RDY_EN
+ *
+ * @return  INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t inv_get_interrupt_status(unsigned char intPin,
+                                     unsigned char *status)
+{
+    INVENSENSE_FUNC_START;
+
+    inv_error_t result;
+
+    switch (intPin) {
+
+    case INTPIN_MPU:
+            /*---- return the MPU interrupt status ----*/
+        result = inv_serial_read(sMLSLHandle, mldlCfg.addr,
+                                 MPUREG_INT_STATUS, 1, status);
+        break;
+
+    default:
+        result = INV_ERROR_INVALID_PARAMETER;
+        break;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief   query the current status of an interrupt source.
+ *  @param   srcIndex
+ *              index of the interrupt source.
+ *              Currently the only source supported is INTPIN_MPU.
+ *
+ *  @return  1 if the interrupt has been triggered.
+ */
+unsigned char inv_get_interrupt_trigger(unsigned char srcIndex)
+{
+    INVENSENSE_FUNC_START;
+    return intTrigger[srcIndex];
+}
+
+/**
+ * @brief clear the 'triggered' status for an interrupt source.
+ * @param srcIndex
+ *          index of the interrupt source.
+ *          Currently only INTPIN_MPU is supported.
+ */
+void inv_clear_interrupt_trigger(unsigned char srcIndex)
+{
+    INVENSENSE_FUNC_START;
+    intTrigger[srcIndex] = 0;
+}
+
+/**
+ * @brief   inv_interrupt_handler function should be called when an interrupt is
+ *          received.  The source parameter identifies which interrupt source
+ *          caused the interrupt. Note that this routine should not be called
+ *          directly from the interrupt service routine.
+ *
+ * @param   intSource   MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1,
+ *                      INTSRC_AUX2, or INT_SRC_TIMER.
+ *
+ * @return  Zero if the command is successful; an error code otherwise.
+ */
+inv_error_t inv_interrupt_handler(unsigned char intSource)
+{
+    INVENSENSE_FUNC_START;
+    /*---- range check ----*/
+    if (intSource >= NUM_OF_INTSOURCES) {
+        return INV_ERROR;
+    }
+
+    /*---- save source of interrupt ----*/
+    intTrigger[intSource] = INT_TRIGGERED;
+
+#ifdef ML_USE_DMP_SIM
+    if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) {
+        MLSimHWDataInput();
+    }
+#endif
+
+    return INV_SUCCESS;
+}
+
+/***************************/
+        /**@}*//* end of defgroup */
+/***************************/
diff --git a/60xx/mlsdk/mllite/mldl.h b/60xx/mlsdk/mllite/mldl.h
new file mode 100644
index 0000000..961d860
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mldl_cfg.h b/60xx/mlsdk/mllite/mldl_cfg.h
new file mode 100644
index 0000000..b4914e2
--- /dev/null
+++ b/60xx/mlsdk/mllite/mldl_cfg.h
@@ -0,0 +1,336 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.h
+ *      @brief  The Motion Library Driver Layer Configuration header file.
+ */
+
+#ifndef __MLDL_CFG_H__
+#define __MLDL_CFG_H__
+
+#include "mltypes.h"
+#include "mlsl.h"
+#include <linux/mpu.h>
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+#    include "mpu6050a2.h"
+#elif defined CONFIG_MPU_SENSORS_MPU6050B1
+#    include "mpu6050b1.h"
+#elif defined CONFIG_MPU_SENSORS_MPU3050
+#  include "mpu3050.h"
+#else
+#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
+#endif
+
+#include "log.h"
+
+/*************************************************************************
+ *  Sensors
+ *************************************************************************/
+
+#define INV_X_GYRO			(0x0001)
+#define INV_Y_GYRO			(0x0002)
+#define INV_Z_GYRO			(0x0004)
+#define INV_DMP_PROCESSOR		(0x0008)
+
+#define INV_X_ACCEL			(0x0010)
+#define INV_Y_ACCEL			(0x0020)
+#define INV_Z_ACCEL			(0x0040)
+
+#define INV_X_COMPASS			(0x0080)
+#define INV_Y_COMPASS			(0x0100)
+#define INV_Z_COMPASS			(0x0200)
+
+#define INV_X_PRESSURE			(0x0300)
+#define INV_Y_PRESSURE			(0x0800)
+#define INV_Z_PRESSURE			(0x1000)
+
+#define INV_TEMPERATURE			(0x2000)
+#define INV_TIME			(0x4000)
+
+#define INV_THREE_AXIS_GYRO		(0x000F)
+#define INV_THREE_AXIS_ACCEL		(0x0070)
+#define INV_THREE_AXIS_COMPASS		(0x0380)
+#define INV_THREE_AXIS_PRESSURE		(0x1C00)
+
+#define INV_FIVE_AXIS			(0x007B)
+#define INV_SIX_AXIS_GYRO_ACCEL		(0x007F)
+#define INV_SIX_AXIS_ACCEL_COMPASS	(0x03F0)
+#define INV_NINE_AXIS			(0x03FF)
+#define INV_ALL_SENSORS			(0x7FFF)
+
+#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
+
+/* -------------------------------------------------------------------------- */
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+	/* MPU related configuration */
+	unsigned long requested_sensors;
+	unsigned char ignore_system_suspend;
+	unsigned char addr;
+	unsigned char int_config;
+	unsigned char ext_sync;
+	unsigned char full_scale;
+	unsigned char lpf;
+	unsigned char clk_src;
+	unsigned char divider;
+	unsigned char dmp_enable;
+	unsigned char fifo_enable;
+	unsigned char dmp_cfg1;
+	unsigned char dmp_cfg2;
+	unsigned char offset_tc[GYRO_NUM_AXES];
+	unsigned short offset[GYRO_NUM_AXES];
+	unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE];
+
+	/* MPU Related stored status and info */
+	unsigned char product_revision;
+	unsigned char silicon_revision;
+	unsigned char product_id;
+	unsigned short gyro_sens_trim;
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+	unsigned short accel_sens_trim;
+#endif
+
+	/* Driver/Kernel related state information */
+	int gyro_is_bypassed;
+	int i2c_slaves_enabled;
+	int dmp_is_running;
+	int gyro_is_suspended;
+	int accel_is_suspended;
+	int compass_is_suspended;
+	int pressure_is_suspended;
+	int gyro_needs_reset;
+
+	/* Slave related information */
+	struct ext_slave_descr *accel;
+	struct ext_slave_descr *compass;
+	struct ext_slave_descr *pressure;
+
+	/* Platform Data */
+	struct mpu_platform_data *pdata;
+};
+
+/* -------------------------------------------------------------------------- */
+
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+		 void *mlsl_handle,
+		 void *accel_handle,
+		 void *compass_handle,
+		 void *pressure_handle);
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+		  void *mlsl_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle);
+int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   unsigned long sensors);
+int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    unsigned long sensors);
+
+/* Slave Read functions */
+int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
+		       void *gyro_handle,
+		       void *slave_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data);
+static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg,
+				     void *gyro_handle,
+				     void *accel_handle, unsigned char *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(mldl_cfg, gyro_handle, accel_handle,
+				  mldl_cfg->accel, &mldl_cfg->pdata->accel,
+				  data);
+}
+
+static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg,
+				       void *gyro_handle,
+				       void *compass_handle,
+				       unsigned char *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(mldl_cfg, gyro_handle, compass_handle,
+				  mldl_cfg->compass, &mldl_cfg->pdata->compass,
+				  data);
+}
+
+static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg,
+					void *gyro_handle,
+					void *pressure_handle,
+					unsigned char *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(mldl_cfg, gyro_handle, pressure_handle,
+				  mldl_cfg->pressure,
+				  &mldl_cfg->pdata->pressure, data);
+}
+
+/* Slave Config functions */
+int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 void *slave_handle,
+			 struct ext_slave_config *data,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata);
+static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg,
+				       void *gyro_handle,
+				       void *accel_handle,
+				       struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(mldl_cfg, gyro_handle, accel_handle, data,
+				    mldl_cfg->accel, &mldl_cfg->pdata->accel);
+}
+
+static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg,
+					 void *gyro_handle,
+					 void *compass_handle,
+					 struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(mldl_cfg, gyro_handle, compass_handle, data,
+				    mldl_cfg->compass,
+				    &mldl_cfg->pdata->compass);
+}
+
+static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg,
+					  void *gyro_handle,
+					  void *pressure_handle,
+					  struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(mldl_cfg, gyro_handle, pressure_handle,
+				    data, mldl_cfg->pressure,
+				    &mldl_cfg->pdata->pressure);
+}
+
+/* Slave get config functions */
+int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
+			     void *gyro_handle,
+			     void *slave_handle,
+			     struct ext_slave_config *data,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata);
+
+static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg,
+					   void *gyro_handle,
+					   void *accel_handle,
+					   struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, accel_handle,
+					data, mldl_cfg->accel,
+					&mldl_cfg->pdata->accel);
+}
+
+static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg,
+					     void *gyro_handle,
+					     void *compass_handle,
+					     struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, compass_handle,
+					data, mldl_cfg->compass,
+					&mldl_cfg->pdata->compass);
+}
+
+static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg,
+					      void *gyro_handle,
+					      void *pressure_handle,
+					      struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(mldl_cfg, gyro_handle,
+					pressure_handle, data,
+					mldl_cfg->pressure,
+					&mldl_cfg->pdata->pressure);
+}
+
+/* -------------------------------------------------------------------------- */
+
+static inline long inv_mpu_get_sampling_rate_hz(struct mldl_cfg *mldl_cfg)
+{
+	if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
+		return 8000L / (mldl_cfg->divider + 1);
+	else
+		return 1000L / (mldl_cfg->divider + 1);
+}
+
+static inline long inv_mpu_get_sampling_period_us(struct mldl_cfg *mldl_cfg)
+{
+	if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
+		return (long) (1000000L * (mldl_cfg->divider + 1)) / 8000L;
+	else
+		return (long) (1000000L * (mldl_cfg->divider + 1)) / 1000L;
+}
+
+#endif				/* __MLDL_CFG_H__ */
+
+/**
+ *@}
+ */
diff --git a/60xx/mlsdk/mllite/mldl_cfg_mpu.c b/60xx/mlsdk/mllite/mldl_cfg_mpu.c
new file mode 100644
index 0000000..3d4496e
--- /dev/null
+++ b/60xx/mlsdk/mllite/mldl_cfg_mpu.c
@@ -0,0 +1,477 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
+ *
+ *****************************************************************************/
+
+/** 
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg_mpu.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <stddef.h>
+#include "mldl_cfg.h"
+#include "mlsl.h"
+#include "mpu.h"
+
+#ifdef LINUX
+#include <sys/ioctl.h>
+#endif
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+
+/* ---------------------- */
+/* -  Static Functions. - */
+/* ---------------------- */
+void mpu_print_cfg(struct mldl_cfg * mldl_cfg)
+{
+    struct mpu_platform_data   *pdata   = mldl_cfg->pdata;
+    struct ext_slave_platform_data *accel   = &mldl_cfg->pdata->accel;
+    struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass;
+    struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure;
+
+    MPL_LOGD("mldl_cfg.addr             = %02x\n", mldl_cfg->addr);
+    MPL_LOGD("mldl_cfg.int_config       = %02x\n", mldl_cfg->int_config);
+    MPL_LOGD("mldl_cfg.ext_sync         = %02x\n", mldl_cfg->ext_sync);
+    MPL_LOGD("mldl_cfg.full_scale       = %02x\n", mldl_cfg->full_scale);
+    MPL_LOGD("mldl_cfg.lpf              = %02x\n", mldl_cfg->lpf);
+    MPL_LOGD("mldl_cfg.clk_src          = %02x\n", mldl_cfg->clk_src);
+    MPL_LOGD("mldl_cfg.divider          = %02x\n", mldl_cfg->divider);
+    MPL_LOGD("mldl_cfg.dmp_enable       = %02x\n", mldl_cfg->dmp_enable);
+    MPL_LOGD("mldl_cfg.fifo_enable      = %02x\n", mldl_cfg->fifo_enable);
+    MPL_LOGD("mldl_cfg.dmp_cfg1         = %02x\n", mldl_cfg->dmp_cfg1);
+    MPL_LOGD("mldl_cfg.dmp_cfg2         = %02x\n", mldl_cfg->dmp_cfg2);
+    MPL_LOGD("mldl_cfg.offset_tc[0]     = %02x\n", mldl_cfg->offset_tc[0]);
+    MPL_LOGD("mldl_cfg.offset_tc[1]     = %02x\n", mldl_cfg->offset_tc[1]);
+    MPL_LOGD("mldl_cfg.offset_tc[2]     = %02x\n", mldl_cfg->offset_tc[2]);
+    MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision);
+    MPL_LOGD("mldl_cfg.product_id       = %02x\n", mldl_cfg->product_id);
+    MPL_LOGD("mldl_cfg.gyro_sens_trim   = %02x\n", mldl_cfg->gyro_sens_trim);
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+    MPL_LOGD("mldl_cfg.accel_sens_trim   = %02x\n", mldl_cfg->accel_sens_trim);
+#endif
+
+    if (mldl_cfg->accel) {
+        MPL_LOGD("slave_accel->suspend      = %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,
+                 void *compass_handle,
+                 void *pressure_handle)
+{
+    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,
+		  void *compass_handle,
+		  void *pressure_handle)
+{
+    int result = INV_SUCCESS;
+
+    result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
+                             INV_ALL_SENSORS);
+    return result;
+}
+
+int inv_mpu_resume(struct mldl_cfg* mldl_cfg, 
+                   void *mlsl_handle, 
+                   void *accel_handle, 
+                   void *compass_handle, 
+                   void *pressure_handle,
+                   unsigned long sensors)
+{
+    int result;
+    
+    mldl_cfg->requested_sensors = sensors;
+    result = ioctl((int)(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,
+                    void *compass_handle,
+                    void *pressure_handle,
+                    unsigned long sensors)
+{
+    int result;
+    unsigned long requested = mldl_cfg->requested_sensors;
+
+    mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
+    //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__,
+    //         mldl_cfg->requested_sensors);
+
+    result = ioctl((int)(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,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata,
+		unsigned char *data)
+{
+    int result;
+    if (!mldl_cfg || !gyro_handle || !data || !slave) {
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    switch (slave->type) {
+    case EXT_SLAVE_TYPE_ACCELEROMETER:
+        result = ioctl((int)(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,
+                     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,
+                         struct ext_slave_config *data,
+                         struct ext_slave_descr *slave,
+                         struct ext_slave_platform_data *pdata)
+{
+    int result;
+    if (!mldl_cfg || !data || !slave) {
+        LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+    switch (slave->type) {
+    case EXT_SLAVE_TYPE_ACCELEROMETER:
+        result = ioctl((int)(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/60xx/mlsdk/mllite/mldmp.c b/60xx/mlsdk/mllite/mldmp.c
new file mode 100644
index 0000000..7381dd4
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mldmp.h b/60xx/mlsdk/mllite/mldmp.h
new file mode 100644
index 0000000..f519798
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlinclude.h b/60xx/mlsdk/mllite/mlinclude.h
new file mode 100644
index 0000000..dcbe904
--- /dev/null
+++ b/60xx/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  typedef int invensensePutFunctionCallsHere
+
+#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/60xx/mlsdk/mllite/mlstates.c b/60xx/mlsdk/mllite/mlstates.c
new file mode 100644
index 0000000..8ebb086
--- /dev/null
+++ b/60xx/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 };
+
+/* --------------- */
+/* -  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/60xx/mlsdk/mllite/mlstates.h b/60xx/mlsdk/mllite/mlstates.h
new file mode 100644
index 0000000..cbaa610
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/mlsupervisor.c b/60xx/mlsdk/mllite/mlsupervisor.c
new file mode 100644
index 0000000..139297f
--- /dev/null
+++ b/60xx/mlsdk/mllite/mlsupervisor.c
@@ -0,0 +1,597 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $
+ *
+ *****************************************************************************/
+
+/**
+ *  @defgroup   ML_SUPERVISOR
+ *  @brief      Basic sensor fusion supervisor functionalities.
+ *
+ *  @{
+ *      @file   mlsupervisor.c
+ *      @brief  Basic sensor fusion supervisor functionalities.
+ */
+
+#include "ml.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "mltypes.h"
+#include "mlinclude.h"
+#include "compass.h"
+#include "pressure.h"
+#include "dmpKey.h"
+#include "dmpDefault.h"
+#include "mlstates.h"
+#include "mlFIFO.h"
+#include "mlFIFOHW.h"
+#include "mlMathFunc.h"
+#include "mlsupervisor.h"
+#include "mlmath.h"
+
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-sup"
+
+static unsigned long lastCompassTime = 0;
+static unsigned long polltime = 0;
+static unsigned long coiltime = 0;
+static int accCount = 0;
+static int compassCalStableCount = 0;
+static int compassCalCount = 0;
+static int coiltimerstart = 0;
+static unsigned long disturbtime = 0;
+static int disturbtimerstart = 0;
+
+static yas_filter_if_s f;
+static yas_filter_handle_t handle;
+
+#define SUPERVISOR_DEBUG 0
+
+struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 };
+
+/**
+ *  @brief  This initializes all variables that should be reset on
+ */
+void inv_init_sensor_fusion_supervisor(void)
+{
+    lastCompassTime = 0;
+    polltime = 0;
+    inv_obj.acc_state = SF_STARTUP_SETTLE;
+    accCount = 0;
+    compassCalStableCount = 0;
+    compassCalCount = 0;
+
+    yas_filter_init(&f);
+    f.init(&handle);
+
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+	defined CONFIG_MPU_SENSORS_MPU6050B1
+    if (inv_compass_present()) {
+        struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+        if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) {
+            (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT);
+        }
+    }
+#endif
+
+    if (ml_supervisor_cb.supervisor_reset_func != NULL) {
+        ml_supervisor_cb.supervisor_reset_func();
+    }
+}
+
+static int MLUpdateCompassCalibration3DOF(int command, long *data,
+                                          unsigned long deltaTime)
+{
+    INVENSENSE_FUNC_START;
+    int retValue = INV_SUCCESS;
+    static float m[10][10] = { {0} };
+    float mInv[10][10] = { {0} };
+    float mTmp[10][10] = { {0} };
+    static float xTransY[4] = { 0 };
+    float magSqr = 0;
+    float inpData[3] = { 0 };
+    int i, j;
+    int a, b;
+    float d;
+
+    switch (command) {
+    case CAL_ADD_DATA:
+        inpData[0] = (float)data[0];
+        inpData[1] = (float)data[1];
+        inpData[2] = (float)data[2];
+        m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]);
+        m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]);
+        m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]);
+        m[0][3] += (-2 * inpData[0]);
+        m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]);
+        m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]);
+        m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]);
+        m[1][3] += (-2 * inpData[1]);
+        m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]);
+        m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]);
+        m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]);
+        m[2][3] += (-2 * inpData[2]);
+        m[3][0] += (-2 * inpData[0]);
+        m[3][1] += (-2 * inpData[1]);
+        m[3][2] += (-2 * inpData[2]);
+        m[3][3] += 1.0f;
+        magSqr =
+            inpData[0] * inpData[0] + inpData[1] * inpData[1] +
+            inpData[2] * inpData[2];
+        xTransY[0] += (-2 * inpData[0]) * magSqr;
+        xTransY[1] += (-2 * inpData[1]) * magSqr;
+        xTransY[2] += (-2 * inpData[2]) * magSqr;
+        xTransY[3] += magSqr;
+        break;
+    case CAL_RUN:
+        a = 4;
+        b = a;
+        for (i = 0; i < b; i++) {
+            for (j = 0; j < b; j++) {
+                a = b;
+                inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j);
+                mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a);
+            }
+        }
+        a = b;
+        d = inv_matrix_det(&m[0][0], &a);
+        if (d == 0) {
+            return INV_ERROR;
+        }
+        for (i = 0; i < 3; i++) {
+            float tmp = 0;
+            for (j = 0; j < 4; j++) {
+                tmp += mInv[j][i] / d * xTransY[j];
+            }
+            inv_obj.compass_test_bias[i] =
+                -(long)(tmp * inv_obj.compass_sens / 16384.0f);
+        }
+        break;
+    case CAL_RESET:
+        for (i = 0; i < 4; i++) {
+            for (j = 0; j < 4; j++) {
+                m[i][j] = 0;
+            }
+            xTransY[i] = 0;
+        }
+    default:
+        break;
+    }
+    return retValue;
+}
+
+/**
+ * Entry point for Sensor Fusion operations
+ * @internal
+ * @param magFB magnetormeter FB
+ * @param accSF accelerometer SF
+ */
+static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF,
+                                            unsigned long deltaTime)
+{
+    static long prevCompassBias[3] = { 0 };
+    static long magMax[3] = {
+        -1073741824L,
+        -1073741824L,
+        -1073741824L
+    };
+    static long magMin[3] = {
+        1073741824L,
+        1073741824L,
+        1073741824L
+    };
+    int gyroMag;
+    long accMag;
+    inv_error_t result;
+    int i;
+
+    if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) {
+        ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime);
+    }
+
+    gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT;
+    accMag = inv_accel_sum_of_sqr();
+
+    // Scaling below assumes certain scaling.
+#if ACC_MAG_SQR_SHIFT != 16
+#error
+#endif
+
+    if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) {
+        result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) {
+        //Most basic compass calibration, used only with lite MPL
+        if (inv_obj.resetting_compass == 1) {
+            for (i = 0; i < 3; i++) {
+                magMax[i] = -1073741824L;
+                magMin[i] = 1073741824L;
+                prevCompassBias[i] = 0;
+            }
+            compassCalStableCount = 0;
+            compassCalCount = 0;
+            inv_obj.resetting_compass = 0;
+        }
+        if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) {
+            if (compassCalStableCount < 1000) {
+                for (i = 0; i < 3; i++) {
+                    if (inv_obj.compass_sensor_data[i] > magMax[i]) {
+                        magMax[i] = inv_obj.compass_sensor_data[i];
+                    }
+                    if (inv_obj.compass_sensor_data[i] < magMin[i]) {
+                        magMin[i] = inv_obj.compass_sensor_data[i];
+                    }
+                }
+                MLUpdateCompassCalibration3DOF(CAL_ADD_DATA,
+                                               inv_obj.compass_sensor_data,
+                                               deltaTime);
+                compassCalStableCount += deltaTime;
+                for (i = 0; i < 3; i++) {
+                    if (magMax[i] - magMin[i] <
+                        (long long)40 * 1073741824 / inv_obj.compass_sens) {
+                        compassCalStableCount = 0;
+                    }
+                }
+            } else {
+                if (compassCalStableCount >= 1000) {
+                    if (MLUpdateCompassCalibration3DOF
+                        (CAL_RUN, inv_obj.compass_sensor_data,
+                         deltaTime) == INV_SUCCESS) {
+                        inv_obj.got_compass_bias = 1;
+                        inv_obj.compass_accuracy = 3;
+                        for(i=0; i<3; i++) {
+                            inv_obj.compass_bias_error[i] = 35;
+                        }
+                        if (inv_obj.compass_state == SF_UNCALIBRATED)
+                            inv_obj.compass_state = SF_STARTUP_SETTLE;
+                        inv_set_compass_bias(inv_obj.compass_test_bias);
+                    }
+                    compassCalCount = 0;
+                    compassCalStableCount = 0;
+                    for (i = 0; i < 3; i++) {
+                        magMax[i] = -1073741824L;
+                        magMin[i] = 1073741824L;
+                        prevCompassBias[i] = 0;
+                    }
+                    MLUpdateCompassCalibration3DOF(CAL_RESET,
+                                                   inv_obj.compass_sensor_data,
+                                                   deltaTime);
+                }
+            }
+        }
+        compassCalCount += deltaTime;
+        if (compassCalCount > 20000) {
+            compassCalCount = 0;
+            compassCalStableCount = 0;
+            for (i = 0; i < 3; i++) {
+                magMax[i] = -1073741824L;
+                magMin[i] = 1073741824L;
+                prevCompassBias[i] = 0;
+            }
+            MLUpdateCompassCalibration3DOF(CAL_RESET,
+                                           inv_obj.compass_sensor_data,
+                                           deltaTime);
+        }
+    }
+
+    if (inv_obj.acc_state != SF_STARTUP_SETTLE) {
+        if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) {
+            inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing
+            accCount = 0;
+        } else {
+            if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214)
+                && ((inv_obj.acc_state == SF_DISTURBANCE)
+                    || (inv_obj.acc_state == SF_SLOW_SETTLE))) {
+                accCount += deltaTime;
+                if (accCount > 0) {
+                    inv_obj.acc_state = SF_FAST_SETTLE;
+                    accCount = 0;
+                }
+            } else {
+                if (inv_obj.acc_state == SF_DISTURBANCE) {
+                    accCount += deltaTime;
+                    if (accCount > 500) {
+                        inv_obj.acc_state = SF_SLOW_SETTLE;
+                        accCount = 0;
+                    }
+                } else if (inv_obj.acc_state == SF_SLOW_SETTLE) {
+                    accCount += deltaTime;
+                    if (accCount > 1000) {
+                        inv_obj.acc_state = SF_NORMAL;
+                        accCount = 0;
+                    }
+                } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
+                    accCount += deltaTime;
+                    if (accCount > 250) {
+                        inv_obj.acc_state = SF_NORMAL;
+                        accCount = 0;
+                    }
+                }
+            }
+        }
+    }
+    if (inv_obj.acc_state == SF_STARTUP_SETTLE) {
+        accCount += deltaTime;
+        if (accCount > 50) {
+            *accSF = 1073741824;    //Startup settling
+            inv_obj.acc_state = SF_NORMAL;
+            accCount = 0;
+        }
+    } else if ((inv_obj.acc_state == SF_NORMAL)
+               || (inv_obj.acc_state == SF_SLOW_SETTLE)) {
+        *accSF = inv_obj.accel_sens * 64;   //Normal
+    } else if ((inv_obj.acc_state == SF_DISTURBANCE)) {
+        *accSF = inv_obj.accel_sens * 64;   //Don't use accel (should be 0)
+    } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
+        *accSF = inv_obj.accel_sens * 512;  //Amplify accel
+    }
+    if (!inv_get_gyro_present()) {
+        *accSF = inv_obj.accel_sens * 128;
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Entry point for software sensor fusion operations.
+ *          Manages hardware interaction, calls sensor fusion supervisor for
+ *          bias calculation.
+ *  @return error code. INV_SUCCESS if no error occurred.
+ */
+inv_error_t inv_accel_compass_supervisor(void)
+{
+    inv_error_t result;
+    int adjustSensorFusion = 0;
+    long accSF = 1073741824;
+    static double magFB = 0;
+    long magSensorData[3];
+    float fcin[3];
+    float fcout[3];
+    
+
+    if (inv_compass_present()) {    /* check for compass data */
+        int i, j;
+        long long tmp[3] = { 0 };
+        long long tmp64 = 0;
+        unsigned long ctime = inv_get_tick_count();
+        if (((inv_get_compass_id() == COMPASS_ID_AK8975) &&
+             ((ctime - polltime) > 20)) ||
+            (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max
+            if (SUPERVISOR_DEBUG) {
+                MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n");
+                MPL_LOGV("delta time = %ld\n", ctime - polltime);
+            }
+            polltime = ctime;
+            result = inv_get_compass_data(magSensorData);
+            /* external slave wants the data even if there is an error */
+            if (result && !inv_obj.external_slave_callback) {
+                if (SUPERVISOR_DEBUG) {
+                    MPL_LOGW("inv_get_compass_data returned %d\n", result);
+                }
+            } else {
+                unsigned long compassTime = inv_get_tick_count();
+                unsigned long deltaTime = 1;
+
+                if (result == INV_SUCCESS) {
+                    if (lastCompassTime != 0) {
+                        deltaTime = compassTime - lastCompassTime;
+                    }
+                    lastCompassTime = compassTime;
+                    adjustSensorFusion = 1;
+                    if (inv_obj.got_init_compass_bias == 0) {
+                        inv_obj.got_init_compass_bias = 1;
+                        for (i = 0; i < 3; i++) {
+                            inv_obj.init_compass_bias[i] = magSensorData[i];
+                        }
+                    }
+                    for (i = 0; i < 3; i++) {
+                        inv_obj.compass_sensor_data[i] = (long)magSensorData[i];
+                        inv_obj.compass_sensor_data[i] -=
+                            inv_obj.init_compass_bias[i];
+                        tmp[i] = ((long long)inv_obj.compass_sensor_data[i])
+                            * inv_obj.compass_sens / 16384;
+                        tmp[i] -= inv_obj.compass_bias[i];
+                        tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L;
+                    }
+                    for (i = 0; i < 3; i++) {
+                        tmp64 = 0;
+                        for (j = 0; j < 3; j++) {
+                            tmp64 += (long long) tmp[j] *
+                                inv_obj.compass_cal[i * 3 + j];
+                        }
+                        inv_obj.compass_calibrated_data[i] =
+                            (long) (tmp64 / inv_obj.compass_sens);
+                    }
+                    //Additions:
+                    if ((inv_obj.compass_state == 1) &&
+                            (inv_obj.compass_overunder == 0)) {
+                        if (disturbtimerstart == 0) {
+                            disturbtimerstart = 1;
+                            disturbtime = ctime;
+                        }
+                    } else {
+                        disturbtimerstart = 0;
+                    }
+
+                    if (inv_obj.compass_overunder) {
+                        if (coiltimerstart == 0) {
+                            coiltimerstart = 1;
+                            coiltime = ctime;
+                        }
+                    }
+                    if (coiltimerstart == 1) {
+                        if (ctime - coiltime > 3000) {
+                            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
+                            inv_set_compass_offset();
+                            inv_reset_compass_calibration();
+                            coiltimerstart = 0;
+                        }
+                    }
+
+                    if (disturbtimerstart == 1) {
+                        if (ctime - disturbtime > 10000) {
+                            inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
+                            inv_set_compass_offset();
+                            inv_reset_compass_calibration();
+                            MPL_LOGI("Compass reset! inv_reset_compass_calibration \n");
+                            disturbtimerstart = 0;
+                        }
+                    }
+                }
+                if (inv_obj.external_slave_callback) {
+                    result = inv_obj.external_slave_callback(&inv_obj);
+                    if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                    }
+                }
+
+#ifdef APPLY_COMPASS_FILTER
+                if (inv_get_compass_id() == COMPASS_ID_YAS530)
+                {
+                    fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f);
+                    fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f);
+                    fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f);
+
+                    f.update(&handle, fcin, fcout);
+
+                    inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f);
+                    inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f);
+                    inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f);
+                }
+#endif
+
+                if (SUPERVISOR_DEBUG) {
+                    MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n",
+                             (float)inv_obj.compass_calibrated_data[0] /
+                             65536.f,
+                             (float)inv_obj.compass_calibrated_data[1] /
+                             65536.f,
+                             (float)inv_obj.compass_calibrated_data[2] /
+                             65536.f);
+                }
+                magFB = 1.0;
+                adjustSensorFusion = 1;
+                result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                    return result;
+                }
+            }
+        }
+    } else {
+        //No compass, but still modify accel
+        unsigned long ctime = inv_get_tick_count();
+        if (polltime == 0 || ((ctime - polltime) > 80)) {   // at the beginning AND every 1/8 second
+            unsigned long deltaTime = 1;
+            adjustSensorFusion = 1;
+            magFB = 0;
+            if (polltime != 0) {
+                deltaTime = ctime - polltime;
+            }
+            MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
+            polltime = ctime;
+        }
+    }
+    if (adjustSensorFusion == 1) {
+        unsigned char regs[4];
+        static int prevAccSF = 1;
+
+        if (accSF != prevAccSF) {
+            regs[0] = (unsigned char)((accSF >> 24) & 0xff);
+            regs[1] = (unsigned char)((accSF >> 16) & 0xff);
+            regs[2] = (unsigned char)((accSF >> 8) & 0xff);
+            regs[3] = (unsigned char)(accSF & 0xff);
+            result = inv_set_mpu_memory(KEY_D_0_96, 4, regs);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            prevAccSF = accSF;
+        }
+    }
+
+    if (ml_supervisor_cb.accel_compass_fusion_func != NULL)
+        ml_supervisor_cb.accel_compass_fusion_func(magFB);
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Entry point for software sensor fusion operations.
+ *          Manages hardware interaction, calls sensor fusion supervisor for
+ *          bias calculation.
+ *  @return INV_SUCCESS or non-zero error code on error.
+ */
+inv_error_t inv_pressure_supervisor(void)
+{
+    long pressureSensorData[1];
+    static unsigned long pressurePolltime = 0;
+    if (inv_pressure_present()) {   /* check for pressure data */
+        unsigned long ctime = inv_get_tick_count();
+        if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second
+            if (SUPERVISOR_DEBUG) {
+                MPL_LOGV("Fetch pressure data\n");
+                MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime);
+            }
+            pressurePolltime = ctime;
+            if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) {
+                inv_obj.pressure = pressureSensorData[0];
+            }
+        }
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  Resets the magnetometer calibration algorithm.
+ *  @return INV_SUCCESS if successful, or non-zero error code otherwise.
+ */
+inv_error_t inv_reset_compass_calibration(void)
+{
+    if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) {
+        if (ml_supervisor_cb.reset_advanced_compass_func != NULL)
+            ml_supervisor_cb.reset_advanced_compass_func();
+    }
+    MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1);
+
+    inv_obj.compass_bias_error[0] = P_INIT;
+    inv_obj.compass_bias_error[1] = P_INIT;
+    inv_obj.compass_bias_error[2] = P_INIT;
+    inv_obj.compass_accuracy = 0;
+
+    inv_obj.got_compass_bias = 0;
+    inv_obj.got_init_compass_bias = 0;
+    inv_obj.compass_state = SF_UNCALIBRATED;
+    inv_obj.resetting_compass = 1;
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @}
+ */
diff --git a/60xx/mlsdk/mllite/mlsupervisor.h b/60xx/mlsdk/mllite/mlsupervisor.h
new file mode 100644
index 0000000..62b427e
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/pressure.c b/60xx/mlsdk/mllite/pressure.c
new file mode 100644
index 0000000..9c162ec
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mllite/pressure.h b/60xx/mlsdk/mllite/pressure.h
new file mode 100644
index 0000000..77c5d43
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mlutils/checksum.c b/60xx/mlsdk/mlutils/checksum.c
new file mode 100644
index 0000000..a97477d
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mlutils/checksum.h b/60xx/mlsdk/mlutils/checksum.h
new file mode 100644
index 0000000..4d3f046
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mlutils/mputest.c b/60xx/mlsdk/mlutils/mputest.c
new file mode 100644
index 0000000..ac0fa30
--- /dev/null
+++ b/60xx/mlsdk/mlutils/mputest.c
@@ -0,0 +1,1396 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $
+ *
+ *****************************************************************************/
+
+/*
+ *  MPU Self Test functions
+ *  Version 2.4
+ *  May 13th, 2011
+ */
+
+/**
+ *  @defgroup MPU_SELF_TEST
+ *  @brief  MPU Self Test functions
+ *
+ *  These functions provide an in-site test of the MPU 3xxx chips. The main
+ *      entry point is the inv_mpu_test function.
+ *  This runs the tests (as described in the accompanying documentation) and
+ *      writes a configuration file containing initial calibration data.
+ *  inv_mpu_test returns INV_SUCCESS if the chip passes the tests.
+ *  Otherwise, an error code is returned.
+ *  The functions in this file rely on MLSL and MLOS: refer to the MPL
+ *      documentation for more information regarding the system interface
+ *      files.
+ *
+ *  @{
+ *      @file   mputest.c
+ *      @brief  MPU Self Test routines for assessing gyro sensor status
+ *              after surface mount has happened on the target host platform.
+ */
+
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+#ifdef LINUX
+#include <unistd.h>
+#endif
+
+#include "mpu.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "accel.h"
+#include "mlFIFO.h"
+#include "slave.h"
+#include "ml.h"
+#include "ml_stored_data.h"
+#include "checksum.h"
+
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mpust"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+    Defines
+*/
+
+#define VERBOSE_OUT 0
+
+/*#define TRACK_IDS*/
+
+/*--- Test parameters defaults. See set_test_parameters for more details ---*/
+
+#define DEF_MPU_ADDR             (0x68)        /* I2C address of the mpu     */
+
+#if (USE_SENSE_PATH_TEST == 1)                 /* gyro full scale dps        */
+#define DEF_GYRO_FULLSCALE       (2000)
+#else
+#define DEF_GYRO_FULLSCALE       (250)
+#endif
+
+#define DEF_GYRO_SENS            (32768.f / DEF_GYRO_FULLSCALE)
+                                               /* gyro sensitivity LSB/dps   */
+#define DEF_PACKET_THRESH        (75)          /* 600 ms / 8ms / sample      */
+#define DEF_TOTAL_TIMING_TOL     (.03f)        /* 3% = 2 pkts + 1% proc tol. */
+#define DEF_BIAS_THRESH          (40 * DEF_GYRO_SENS)
+                                               /* 40 dps in LSBs             */
+#define DEF_RMS_THRESH           (0.4f * DEF_GYRO_SENS)
+                                               /* 0.4 dps-rms in LSB-rms     */
+#define DEF_SP_SHIFT_THRESH_CUST (.05f)        /* 5%                         */
+#define DEF_TEST_TIME_PER_AXIS   (600)         /* ms of time spent collecting
+                                                  data for each axis,
+                                                  multiple of 600ms          */
+#define DEF_N_ACCEL_SAMPLES      (20)          /* num of accel samples to
+                                                  average from, if applic.   */
+
+#define ML_INIT_CAL_LEN          (36)          /* length in bytes of
+                                                  calibration data file      */
+
+/*
+    Macros
+*/
+
+#define CHECK_TEST_ERROR(x)                                                 \
+    if (x) {                                                                \
+        MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__);              \
+        return (-1);                                                        \
+    }
+
+#define SHORT_TO_TEMP_C(shrt)         (((shrt+13200.f)/280.f)+35.f)
+
+#define USHORT_TO_CHARS(chr,shrt)     (chr)[0]=(unsigned char)(shrt>>8);    \
+                                      (chr)[1]=(unsigned char)(shrt);
+
+#define UINT_TO_CHARS(chr,ui)         (chr)[0]=(unsigned char)(ui>>24);     \
+                                      (chr)[1]=(unsigned char)(ui>>16);     \
+                                      (chr)[2]=(unsigned char)(ui>>8);      \
+                                      (chr)[3]=(unsigned char)(ui);
+
+#define FLOAT_TO_SHORT(f)             (                                     \
+                                        (fabs(f-(short)f)>=0.5) ? (         \
+                                            ((short)f)+(f<0?(-1):(+1))) :   \
+                                            ((short)f)                      \
+                                      )
+
+#define CHARS_TO_SHORT(d)             ((((short)(d)[0])<<8)+(d)[1])
+#define CHARS_TO_SHORT_SWAPPED(d)     ((((short)(d)[1])<<8)+(d)[0])
+
+#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5]
+
+#define CHECK_NACKS(d)  (                               \
+                         d[0]==0xff && d[1]==0xff &&    \
+                         d[2]==0xff && d[3]==0xff &&    \
+                         d[4]==0xff && d[5]==0xff       \
+                        )
+
+/*
+    Prototypes
+*/
+
+static inv_error_t test_get_data(
+                    void *mlsl_handle,
+                    struct mldl_cfg *mputestCfgPtr,
+                    short *vals);
+
+/*
+    Types
+*/
+typedef struct {
+    float gyro_sens;
+    int gyro_fs;
+    int packet_thresh;
+    float total_timing_tol;
+    int bias_thresh;
+    float rms_threshSq;
+    float sp_shift_thresh;
+    unsigned int test_time_per_axis;
+    unsigned short accel_samples;
+} tTestSetup;
+
+/*
+    Global variables
+*/
+static unsigned char dataout[20];
+static unsigned char dataStore[ML_INIT_CAL_LEN];
+
+static tTestSetup test_setup = {
+    DEF_GYRO_SENS,
+    DEF_GYRO_FULLSCALE,
+    DEF_PACKET_THRESH,
+    DEF_TOTAL_TIMING_TOL,
+    (int)DEF_BIAS_THRESH,
+    DEF_RMS_THRESH * DEF_RMS_THRESH,
+    DEF_SP_SHIFT_THRESH_CUST,
+    DEF_TEST_TIME_PER_AXIS,
+    DEF_N_ACCEL_SAMPLES
+};
+
+static float adjGyroSens;
+static char a_name[3][2] = {"X", "Y", "Z"};
+
+/*
+    NOTE :  modify get_slave_descr parameter below to reflect
+                the DEFAULT accelerometer in use. The accelerometer in use
+                can be modified at run-time using the inv_test_setup_accel API.
+    NOTE :  modify the expected z axis orientation (Z axis pointing
+                upward or downward)
+*/
+
+signed char g_z_sign = +1;
+struct mldl_cfg *mputestCfgPtr = NULL;
+
+#ifndef LINUX
+/**
+ *  @internal
+ *  @brief  usec precision sleep function.
+ *  @param  number of micro seconds (us) to sleep for.
+ */
+static void usleep(unsigned long t)
+{
+    unsigned long start = inv_get_tick_count();
+    while (inv_get_tick_count()-start < t / 1000);
+}
+#endif
+
+/**
+ *  @brief  Modify the self test limits from their default values.
+ *
+ *  @param  slave_addr
+ *              the slave address the MPU device is setup to respond at.
+ *              The default is DEF_MPU_ADDR = 0x68.
+ *  @param  sensitivity
+ *              the read sensitivity of the device in LSB/dps as it is trimmed.
+ *              NOTE :  if using the self test as part of the MPL, the
+ *                      sensitivity the different sensitivity trims are already
+ *                      taken care of.
+ *  @param  p_thresh
+ *              number of packets expected to be received in a 600 ms period.
+ *              Depends on the sampling frequency of choice (set by default to
+ *              125 Hz) and low pass filter cut-off frequency selection (set
+ *              to 42 Hz).
+ *              The default is DEF_PACKET_THRESH = 75 packets.
+ *  @param  total_time_tol
+ *              time skew tolerance, taking into account imprecision in turning
+ *              the FIFO on and off and the processor time imprecision (for
+ *              1 GHz processor).
+ *              The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets.
+ *  @param  bias_thresh
+ *              bias level threshold, the maximun acceptable no motion bias
+ *              for a production quality part.
+ *              The default is DEF_BIAS_THRESH = 40 dps.
+ *  @param  rms_thresh
+ *              the limit standard deviation (=~ RMS) set to assess whether
+ *              the noise level on the part is acceptable.
+ *              The default is DEF_RMS_THRESH = 0.2 dps-rms.
+ *  @param  sp_shift_thresh
+ *              the limit shift applicable to the Sense Path self test
+ *              calculation.
+ */
+void inv_set_test_parameters(unsigned int slave_addr, float sensitivity,
+                             int p_thresh, float total_time_tol,
+                             int bias_thresh, float rms_thresh,
+                             float sp_shift_thresh,
+                             unsigned short accel_samples)
+{
+    mputestCfgPtr->addr = slave_addr;
+    test_setup.gyro_sens = sensitivity;
+    test_setup.gyro_fs = (int)(32768.f / sensitivity);
+    test_setup.packet_thresh = p_thresh;
+    test_setup.total_timing_tol = total_time_tol;
+    test_setup.bias_thresh = bias_thresh;
+    test_setup.rms_threshSq = rms_thresh * rms_thresh;
+    test_setup.sp_shift_thresh = sp_shift_thresh;
+    test_setup.accel_samples = accel_samples;
+}
+
+#define X   (0)
+#define Y   (1)
+#define Z   (2)
+
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+/**
+ *  @brief  Test the gyroscope sensor.
+ *          Implements the core logic of the MPU Self Test.
+ *          Produces the PASS/FAIL result. Loads the calculated gyro biases
+ *          and temperature datum into the corresponding pointers.
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  gyro_biases
+ *              output pointer to store the initial bias calculation provided
+ *              by the MPU Self Test.  Requires 3 elements for gyro X, Y,
+ *              and Z.
+ *  @param  temp_avg
+ *              output pointer to store the initial average temperature as
+ *              provided by the MPU Self Test.
+ *  @param  perform_full_test
+ *              If 1:
+ *              calculates offset, drive frequency, and noise and compare it
+ *              against set thresholds.
+ *              Report also the final result using a bit-mask like error code
+ *              as explained in return value description.
+ *              When 0:
+ *              skip the noise and drive frequency calculation and pass/fail
+ *              assessment; simply calculates the gyro biases.
+ *
+ *  @return 0 on success.
+ *          On error, the return value is a bitmask representing:
+ *          0, 1, 2     Failures with PLLs on X, Y, Z gyros respectively
+ *                      (decimal values will be 1, 2, 4 respectively).
+ *          3, 4, 5     Excessive offset with X, Y, Z gyros respectively
+ *                      (decimal values will be 8, 16, 32 respectively).
+ *          6, 7, 8     Excessive noise with X, Y, Z gyros respectively
+ *                      (decimal values will be 64, 128, 256 respectively).
+ *          9           If any of the RMS noise values is zero, it could be
+ *                      due to a non-functional gyro or FIFO/register failure.
+ *                      (decimal value will be 512).
+ *                      (decimal values will be 1024, 2048, 4096 respectively).
+ */
+int inv_test_gyro_3050(void *mlsl_handle,
+                       short gyro_biases[3], short *temp_avg,
+                       uint_fast8_t perform_full_test)
+{
+    int retVal = 0;
+    inv_error_t result;
+
+    int total_count = 0;
+    int total_count_axis[3] = {0, 0, 0};
+    int packet_count;
+    short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    unsigned char regs[7];
+
+    int temperature;
+    float Avg[3];
+    float RMS[3];
+    int i, j, tmp;
+    char tmpStr[200];
+
+    temperature = 0;
+
+    /* sample rate = 8ms */
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_SMPLRT_DIV, 0x07);
+    CHECK_TEST_ERROR(result);
+
+    regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
+    switch (DEF_GYRO_FULLSCALE) {
+        case 2000:
+            regs[0] |= 0x18;
+            break;
+        case 1000:
+            regs[0] |= 0x10;
+            break;
+        case 500:
+            regs[0] |= 0x08;
+            break;
+        case 250:
+        default:
+            regs[0] |= 0x00;
+            break;
+    }
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_DLPF_FS_SYNC, regs[0]);
+    CHECK_TEST_ERROR(result);
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_INT_CFG, 0x00);
+    CHECK_TEST_ERROR(result);
+
+    /* 1st, timing test */
+    for (j = 0; j < 3; j++) {
+
+        MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
+
+        /* turn on all gyros, use gyro X for clocking
+           Set to Y and Z for 2nd and 3rd iteration */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGM, j + 1);
+        CHECK_TEST_ERROR(result);
+
+        /* wait for 2 ms after switching clock source */
+        usleep(2000);
+
+        /* we will enable XYZ gyro in FIFO and nothing else */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_FIFO_EN2, 0x00);
+        CHECK_TEST_ERROR(result);
+        /* enable/reset FIFO */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
+        CHECK_TEST_ERROR(result);
+
+        tmp = (int)(test_setup.test_time_per_axis / 600);
+        while (tmp-- > 0) {
+            /* enable XYZ gyro in FIFO and nothing else */
+            result = inv_serial_single_write(mlsl_handle,
+                        mputestCfgPtr->addr, MPUREG_FIFO_EN1,
+                        BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+            CHECK_TEST_ERROR(result);
+
+            /* wait for 600 ms for data */
+            usleep(600000);
+
+            /* stop storing gyro in the FIFO */
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_FIFO_EN1, 0x00);
+            CHECK_TEST_ERROR(result);
+
+            /* Getting number of bytes in FIFO */
+            result = inv_serial_read(
+                           mlsl_handle, mputestCfgPtr->addr,
+                           MPUREG_FIFO_COUNTH, 2, dataout);
+            CHECK_TEST_ERROR(result);
+            /* number of 6 B packets in the FIFO */
+            packet_count = CHARS_TO_SHORT(dataout) / 6;
+            sprintf(tmpStr, "Packet Count: %d - ", packet_count);
+
+            if ( abs(packet_count - test_setup.packet_thresh)
+                        <=      /* Within +/- total_timing_tol % range */
+                     test_setup.total_timing_tol * test_setup.packet_thresh) {
+                for (i = 0; i < packet_count; i++) {
+                    /* getting FIFO data */
+                    result = inv_serial_read_fifo(mlsl_handle,
+                                mputestCfgPtr->addr, 6, dataout);
+                    CHECK_TEST_ERROR(result);
+                    x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
+                    y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
+                    z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
+                    if (VERBOSE_OUT) {
+                        MPL_LOGI("Gyros %-4d    : %+13d %+13d %+13d\n",
+                                    total_count + i, x[total_count + i],
+                                    y[total_count + i], z[total_count + i]);
+                    }
+                }
+                total_count += packet_count;
+                total_count_axis[j] += packet_count;
+                sprintf(tmpStr, "%sOK", tmpStr);
+            } else {
+                if (perform_full_test)
+                    retVal |= 1 << j;
+                sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
+            }
+            MPL_LOGI("%s\n", tmpStr);
+        }
+
+        /* remove gyros from FIFO */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_FIFO_EN1, 0x00);
+        CHECK_TEST_ERROR(result);
+
+        /* Read Temperature */
+        result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_TEMP_OUT_H, 2, dataout);
+        CHECK_TEST_ERROR(result);
+        temperature += (short)CHARS_TO_SHORT(dataout);
+    }
+
+    MPL_LOGI("\n");
+    MPL_LOGI("Total %d samples\n", total_count);
+    MPL_LOGI("\n");
+
+    /* 2nd, check bias from X and Y PLL clock source */
+    tmp = total_count != 0 ? total_count : 1;
+    for (i = 0,
+            Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
+         i < total_count; i++) {
+        Avg[X] += 1.f * x[i] / tmp;
+        Avg[Y] += 1.f * y[i] / tmp;
+        Avg[Z] += 1.f * z[i] / tmp;
+    }
+    MPL_LOGI("bias          : %+13.3f %+13.3f %+13.3f (LSB)\n",
+             Avg[X], Avg[Y], Avg[Z]);
+    if (VERBOSE_OUT) {
+        MPL_LOGI("              : %+13.3f %+13.3f %+13.3f (dps)\n",
+                 Avg[X] / adjGyroSens,
+                 Avg[Y] / adjGyroSens,
+                 Avg[Z] / adjGyroSens);
+    }
+    if(perform_full_test) {
+        for (j = 0; j < 3; j++) {
+            if (fabs(Avg[j]) > test_setup.bias_thresh) {
+                MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
+                         "(threshold = %d)\n",
+                         a_name[j], Avg[j], test_setup.bias_thresh);
+                retVal |= 1 << (3+j);
+            }
+        }
+    }
+
+    /* 3rd, check RMS */
+    if (perform_full_test) {
+        for (i = 0,
+                RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
+             i < total_count; i++) {
+            RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]);
+            RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]);
+            RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]);
+        }
+        for (j = 0; j < 3; j++) {
+            if (RMS[j] > test_setup.rms_threshSq * total_count) {
+                MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold "
+                         "(threshold = %.2f)\n",
+                         a_name[j], sqrt(RMS[j] / total_count),
+                         sqrt(test_setup.rms_threshSq));
+                retVal |= 1 << (6+j);
+            }
+        }
+
+        MPL_LOGI("RMS           : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
+                    sqrt(RMS[X] / total_count),
+                    sqrt(RMS[Y] / total_count),
+                    sqrt(RMS[Z] / total_count));
+        if (VERBOSE_OUT) {
+            MPL_LOGI("RMS ^ 2       : %+13.3f %+13.3f %+13.3f\n",
+                        RMS[X] / total_count,
+                        RMS[Y] / total_count,
+                        RMS[Z] / total_count);
+        }
+
+        if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) {
+            /*  If any of the RMS noise value returns zero,
+                then we might have dead gyro or FIFO/register failure,
+                the part is sleeping, or the part is not responsive */
+            retVal |= 1 << 9;
+        }
+    }
+
+    /* 4th, temperature average */
+    temperature /= 3;
+    if (VERBOSE_OUT)
+        MPL_LOGI("Temperature   : %+13.3f %13s %13s (deg. C)\n",
+                 SHORT_TO_TEMP_C(temperature), "", "");
+
+    /* load into final storage */
+    *temp_avg = (short)temperature;
+    gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
+    gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
+    gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
+
+    return retVal;
+}
+
+#else /* CONFIG_MPU_SENSORS_MPU3050 */
+
+/**
+ *  @brief  Test the gyroscope sensor.
+ *          Implements the core logic of the MPU Self Test but does not provide
+ *          a PASS/FAIL output as in the MPU-3050 implementation.
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  gyro_biases
+ *              output pointer to store the initial bias calculation provided
+ *              by the MPU Self Test.  Requires 3 elements for gyro X, Y,
+ *              and Z.
+ *  @param  temp_avg
+ *              output pointer to store the initial average temperature as
+ *              provided by the MPU Self Test.
+ *
+ *  @return 0 on success.
+ *          A non-zero error code on error.
+ */
+int inv_test_gyro_6050(void *mlsl_handle,
+                       short gyro_biases[3], short *temp_avg)
+{
+    inv_error_t result;
+
+    int total_count = 0;
+    int total_count_axis[3] = {0, 0, 0};
+    int packet_count;
+    short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0};
+    unsigned char regs[7];
+
+    int temperature = 0;
+    float Avg[3];
+    int i, j, tmp;
+    char tmpStr[200];
+
+    /* sample rate = 8ms */
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_SMPLRT_DIV, 0x07);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
+    switch (DEF_GYRO_FULLSCALE) {
+        case 2000:
+            regs[0] |= 0x18;
+            break;
+        case 1000:
+            regs[0] |= 0x10;
+            break;
+        case 500:
+            regs[0] |= 0x08;
+            break;
+        case 250:
+        default:
+            regs[0] |= 0x00;
+            break;
+    }
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_CONFIG, regs[0]);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_serial_single_write(
+                mlsl_handle, mputestCfgPtr->addr,
+                MPUREG_INT_ENABLE, 0x00);
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    /* 1st, timing test */
+    for (j = 0; j < 3; j++) {
+        MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
+
+        /* turn on all gyros, use gyro X for clocking
+           Set to Y and Z for 2nd and 3rd iteration */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1));
+#else
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGMT_1, j + 1);
+#endif
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        /* wait for 2 ms after switching clock source */
+        usleep(2000);
+
+        /* enable/reset FIFO */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        tmp = (int)(test_setup.test_time_per_axis / 600);
+        while (tmp-- > 0) {
+            /* enable XYZ gyro in FIFO and nothing else */
+            result = inv_serial_single_write(mlsl_handle,
+                        mputestCfgPtr->addr, MPUREG_FIFO_EN,
+                        BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+
+            /* wait for 600 ms for data */
+            usleep(600000);
+            /* stop storing gyro in the FIFO */
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_FIFO_EN, 0x00);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            /* Getting number of bytes in FIFO */
+            result = inv_serial_read(
+                           mlsl_handle, mputestCfgPtr->addr,
+                           MPUREG_FIFO_COUNTH, 2, dataout);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+            /* number of 6 B packets in the FIFO */
+            packet_count = CHARS_TO_SHORT(dataout) / 6;
+            sprintf(tmpStr, "Packet Count: %d - ", packet_count);
+
+            if (abs(packet_count - test_setup.packet_thresh)
+                        <=      /* Within +/- total_timing_tol % range */
+                     test_setup.total_timing_tol * test_setup.packet_thresh) {
+                for (i = 0; i < packet_count; i++) {
+                    /* getting FIFO data */
+                    result = inv_serial_read_fifo(mlsl_handle,
+                                mputestCfgPtr->addr, 6, dataout);
+                    if (result) {
+                        LOG_RESULT_LOCATION(result);
+                        return result;
+                    }
+                    x[total_count + i] = CHARS_TO_SHORT(&dataout[0]);
+                    y[total_count + i] = CHARS_TO_SHORT(&dataout[2]);
+                    z[total_count + i] = CHARS_TO_SHORT(&dataout[4]);
+                    if (VERBOSE_OUT) {
+                        MPL_LOGI("Gyros %-4d    : %+13d %+13d %+13d\n",
+                                    total_count + i, x[total_count + i],
+                                    y[total_count + i], z[total_count + i]);
+                    }
+                }
+                total_count += packet_count;
+                total_count_axis[j] += packet_count;
+                sprintf(tmpStr, "%sOK", tmpStr);
+            } else {
+                sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
+            }
+            MPL_LOGI("%s\n", tmpStr);
+        }
+
+        /* remove gyros from FIFO */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_FIFO_EN, 0x00);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        /* Read Temperature */
+        result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_TEMP_OUT_H, 2, dataout);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+        temperature += (short)CHARS_TO_SHORT(dataout);
+    }
+
+    MPL_LOGI("\n");
+    MPL_LOGI("Total %d samples\n", total_count);
+    MPL_LOGI("\n");
+
+    /* 2nd, check bias from X and Y PLL clock source */
+    tmp = total_count != 0 ? total_count : 1;
+    for (i = 0,
+            Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f;
+         i < total_count; i++) {
+        Avg[X] += 1.f * x[i] / tmp;
+        Avg[Y] += 1.f * y[i] / tmp;
+        Avg[Z] += 1.f * z[i] / tmp;
+    }
+    MPL_LOGI("bias          : %+13.3f %+13.3f %+13.3f (LSB)\n",
+             Avg[X], Avg[Y], Avg[Z]);
+    if (VERBOSE_OUT) {
+        MPL_LOGI("              : %+13.3f %+13.3f %+13.3f (dps)\n",
+                 Avg[X] / adjGyroSens,
+                 Avg[Y] / adjGyroSens,
+                 Avg[Z] / adjGyroSens);
+    }
+
+    temperature /= 3;
+    if (VERBOSE_OUT)
+        MPL_LOGI("Temperature   : %+13.3f %13s %13s (deg. C)\n",
+                 SHORT_TO_TEMP_C(temperature), "", "");
+
+    /* load into final storage */
+    *temp_avg = (short)temperature;
+    gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]);
+    gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]);
+    gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]);
+
+    return INV_SUCCESS;
+}
+
+#endif /* CONFIG_MPU_SENSORS_MPU3050 */
+
+#ifdef TRACK_IDS
+/**
+ *  @internal
+ *  @brief  Retrieve the unique MPU device identifier from the internal OTP
+ *          bank 0 memory.
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @return 0 on success, a non-zero error code from the serial layer on error.
+ */
+static inv_error_t test_get_mpu_id(void *mlsl_handle)
+{
+    inv_error_t result;
+    unsigned char otp0[8];
+
+
+    result =
+        inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr,
+            (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 |
+            0x00, 6, otp0);
+    if (result)
+        goto close;
+
+    MPL_LOGI("\n");
+    MPL_LOGI("DIE_ID   : %06X\n",
+                ((int)otp0[1] << 8 | otp0[0]) & 0x1fff);
+    MPL_LOGI("WAFER_ID : %06X\n",
+                (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5);
+    MPL_LOGI("A_LOT_ID : %06X\n",
+                ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 |
+                otp0[2]) & 0x3ffff) >> 2);
+    MPL_LOGI("W_LOT_ID : %06X\n",
+                ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2);
+    MPL_LOGI("WP_ID    : %06X\n",
+                ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7);
+    MPL_LOGI("REV_ID   : %06X\n", otp0[6] >> 2);
+    MPL_LOGI("\n");
+
+close:
+    result =
+        inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00);
+    return result;
+}
+#endif /* TRACK_IDS */
+
+/**
+ *  @brief  If requested via inv_test_setup_accel(), test the accelerometer biases
+ *          and calculate the necessary bias correction.
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  bias
+ *              output pointer to store the initial bias calculation provided
+ *              by the MPU Self Test.  Requires 3 elements to store accel X, Y,
+ *              and Z axis bias.
+ *  @param  perform_full_test
+ *              If 1:
+ *              calculates offsets and noise and compare it against set
+ *              thresholds. The final exist status will reflect if any of the
+ *              value is outside of the expected range.
+ *              When 0;
+ *              skip the noise calculation and pass/fail assessment; simply
+ *              calculates the accel biases.
+ *
+ *  @return 0 on success. A non-zero error code on error.
+ */
+int inv_test_accel(void *mlsl_handle,
+                   short *bias, long gravity,
+                   uint_fast8_t perform_full_test)
+{
+    int i;
+
+    short *p_vals;
+    float x = 0.f, y = 0.f, z = 0.f, zg = 0.f;
+    float RMS[3];
+    float accelRmsThresh = 1000000.f; /* enourmous so that the test always
+                                         passes - future deployment */
+    unsigned short res;
+    unsigned long orig_requested_sensors;
+    struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata;
+
+    p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
+
+    /* load the slave descr from the getter */
+    if (mputestPData->accel.get_slave_descr == NULL) {
+        MPL_LOGI("\n");
+        MPL_LOGI("No accelerometer configured\n");
+        return 0;
+    }
+    if (mputestCfgPtr->accel == NULL) {
+        MPL_LOGI("\n");
+        MPL_LOGI("No accelerometer configured\n");
+        return 0;
+    }
+
+    /* resume the accel */
+    orig_requested_sensors = mputestCfgPtr->requested_sensors;
+    mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO;
+    res = inv_mpu_resume(mputestCfgPtr,
+                         mlsl_handle, NULL, NULL, NULL,
+                         mputestCfgPtr->requested_sensors);
+    if(res != INV_SUCCESS)
+        goto accel_error;
+
+    /* wait at least a sample cycle for the
+       accel data to be retrieved by MPU */
+    inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000);
+
+    /* collect the samples  */
+    for(i = 0; i < test_setup.accel_samples; i++) {
+        short *vals = &p_vals[3 * i];
+        if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) {
+            goto accel_error;
+        }
+        x += 1.f * vals[X] / test_setup.accel_samples;
+        y += 1.f * vals[Y] / test_setup.accel_samples;
+        z += 1.f * vals[Z] / test_setup.accel_samples;
+    }
+
+    mputestCfgPtr->requested_sensors = orig_requested_sensors;
+    res = inv_mpu_suspend(mputestCfgPtr,
+                          mlsl_handle, NULL, NULL, NULL,
+                          INV_ALL_SENSORS);
+    if (res != INV_SUCCESS)
+        goto accel_error;
+
+    MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z);
+    if (VERBOSE_OUT) {
+        MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (gee)\n",
+                    x / gravity, y / gravity, z / gravity);
+    }
+
+    bias[0] = FLOAT_TO_SHORT(x);
+    bias[1] = FLOAT_TO_SHORT(y);
+    zg = z - g_z_sign * gravity;
+    bias[2] = FLOAT_TO_SHORT(zg);
+
+    MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
+             bias[0], bias[1], bias[2]);
+    if (VERBOSE_OUT) {
+        MPL_LOGI("Accel correct.: "
+               "%+13.3f %+13.3f %+13.3f (gee)\n",
+                    1.f * bias[0] / gravity,
+                    1.f * bias[1] / gravity,
+                    1.f * bias[2] / gravity);
+    }
+
+    if (perform_full_test) {
+        /* accel RMS - for now the threshold is only indicative */
+        for (i = 0,
+                 RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f;
+             i <  test_setup.accel_samples; i++) {
+            short *vals = &p_vals[3 * i];
+            RMS[X] += (vals[X] - x) * (vals[X] - x);
+            RMS[Y] += (vals[Y] - y) * (vals[Y] - y);
+            RMS[Z] += (vals[Z] - z) * (vals[Z] - z);
+        }
+        for (i = 0; i < 3; i++) {
+            if (RMS[i] >  accelRmsThresh * accelRmsThresh
+                            * test_setup.accel_samples) {
+                MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
+                         "(threshold = %.2f)\n",
+                         a_name[i], sqrt(RMS[i] / test_setup.accel_samples),
+                         accelRmsThresh);
+                goto accel_error;
+            }
+        }
+        MPL_LOGI("RMS           : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
+                 sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES),
+                 sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES),
+                 sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES));
+    }
+
+    return 0; /* success */
+
+accel_error:  /* error */
+    bias[0] = bias[1] = bias[2] = 0;
+    return 1;
+}
+
+/**
+ *  @brief  an user-space substitute of the power management function(s)
+ *          in mldl_cfg.c for self test usage.
+ *          Wake up and sleep the device, whether that is MPU3050, MPU6050A2,
+ *          or MPU6050B1.
+ *  @param  mlsl_handle
+ *              a file handle for the serial communication port used to
+ *              communicate with the MPU device.
+ *  @param  power_level
+ *              the power state to change the device into. Currently only 0 or
+ *              1 are supported, for sleep and full-power respectively.
+ *  @return 0 on success; a non-zero error code on error.
+ */
+static inv_error_t inv_device_power_mgmt(void *mlsl_handle,
+                                         uint_fast8_t power_level)
+{
+    inv_error_t result;
+    static unsigned char pwr_mgm;
+    unsigned char b;
+
+    if (power_level != 0 && power_level != 1) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    if (power_level) {
+        result = inv_serial_read(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGM, 1, &pwr_mgm);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        /* reset */
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET);
+#ifndef CONFIG_MPU_SENSORS_MPU6050A2
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+#endif
+        inv_sleep(5);
+
+        /* re-read power mgmt reg -
+           it may have reset after H_RESET is applied */
+        result = inv_serial_read(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_PWR_MGM, 1, &b);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+
+        /* wake up */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+        if ((b & BITS_PWRSEL) != BITS_PWRSEL) {
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_PWR_MGM, BITS_PWRSEL);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+#else
+        if (pwr_mgm & BIT_SLEEP) {
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_PWR_MGM, 0x00);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+#endif
+        inv_sleep(60);
+
+#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \
+    defined(CONFIG_MPU_SENSORS_MPU6050B1)
+        result = inv_serial_single_write(
+                    mlsl_handle, mputestCfgPtr->addr,
+                    MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+#endif
+    } else {
+        /* restore the power state the part was found in */
+#ifdef CONFIG_MPU_SENSORS_MPU6050A2
+        if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) {
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_PWR_MGM, pwr_mgm);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+#else
+        if (pwr_mgm & BIT_SLEEP) {
+            result = inv_serial_single_write(
+                        mlsl_handle, mputestCfgPtr->addr,
+                        MPUREG_PWR_MGM, pwr_mgm);
+            if (result) {
+                LOG_RESULT_LOCATION(result);
+                return result;
+            }
+        }
+#endif
+    }
+
+    return INV_SUCCESS;
+}
+
+/**
+ *  @brief  The main entry point of the MPU Self Test, triggering the run of
+ *          the single tests, for gyros and accelerometers.
+ *          Prepares the MPU for the test, taking the device out of low power
+ *          state if necessary, switching the MPU secondary I2C interface into
+ *          bypass mode and restoring the original power state at the end of
+ *          the test.
+ *          This function is also responsible for encoding the output of each
+ *          test in the correct format as it is stored on the file/medium of
+ *          choice (according to inv_serial_write_cal() function).
+ *          The format needs to stay perfectly consistent with the one expected
+ *          by the corresponding loader in ml_stored_data.c; currectly the
+ *          loaded in use is inv_load_cal_V1 (record type 1 - initial
+ *          calibration).
+ *
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  provide_result
+ *              If 1:
+ *              perform and analyze the offset, drive frequency, and noise
+ *              calculation and compare it against set threshouds. Report
+ *              also the final result using a bit-mask like error code as
+ *              described in the inv_test_gyro() function.
+ *              When 0:
+ *              skip the noise and drive frequency calculation and pass/fail
+ *              assessment. It simply calculates the gyro and accel biases.
+ *              NOTE: for MPU6050 devices, this parameter is currently
+ *              ignored.
+ *
+ *  @return 0 on success.  A non-zero error code on error.
+ *          Propagates the errors from the tests up to the caller.
+ */
+int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result)
+{
+    int result = 0;
+
+    short temp_avg;
+    short gyro_biases[3] = {0, 0, 0};
+    short accel_biases[3] = {0, 0, 0};
+
+    unsigned long testStart = inv_get_tick_count();
+    long accelSens[3] = {0};
+    int ptr;
+    int tmp;
+    long long lltmp;
+    uint32_t chk;
+
+    MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n",
+                DEF_TEST_TIME_PER_AXIS / 600);
+    MPL_LOGI("\n");
+
+    result = inv_device_power_mgmt(mlsl_handle, TRUE);
+
+#ifdef TRACK_IDS
+    result = test_get_mpu_id(mlsl_handle);
+    if (result != INV_SUCCESS) {
+        MPL_LOGI("Could not read the device's unique ID\n");
+        MPL_LOGI("\n");
+        return result;
+    }
+#endif
+
+    /* adjust the gyro sensitivity according to the gyro_sens_trim value */
+    adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f;
+    test_setup.gyro_fs = (int)(32768.f / adjGyroSens);
+
+    /* collect gyro and temperature data */
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+    result = inv_test_gyro_3050(mlsl_handle,
+                gyro_biases, &temp_avg, provide_result);
+#else
+    MPL_LOGW_IF(provide_result,
+                "Self Test for MPU-6050 devices is for bias correction only: "
+                "no test PASS/FAIL result will be provided\n");
+    result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg);
+#endif
+
+    MPL_LOGI("\n");
+    MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart);
+    if (result)
+        return result;
+
+    /* collect accel data.  if this step is skipped,
+       ensure the array still contains zeros. */
+    if (mputestCfgPtr->accel != NULL) {
+        float fs;
+        RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs);
+        accelSens[0] = (long)(32768L / fs);
+        accelSens[1] = (long)(32768L / fs);
+        accelSens[2] = (long)(32768L / fs);
+#if defined CONFIG_MPU_SENSORS_MPU6050B1
+        if (MPL_PROD_KEY(mputestCfgPtr->product_id,
+                mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) {
+            accelSens[2] /= 2;
+        } else {
+            unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim;
+            accelSens[0] /= trim_corr;
+            accelSens[1] /= trim_corr;
+            accelSens[2] /= trim_corr;
+        }
+#endif
+    } else {
+        /* would be 0, but 1 to avoid divide-by-0 below */
+        accelSens[0] = accelSens[1] = accelSens[2] = 1;
+    }
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+    result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
+                            provide_result);
+#else
+    result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
+                            FALSE);
+#endif
+    if (result)
+        return result;
+
+    result = inv_device_power_mgmt(mlsl_handle, FALSE);
+    if (result)
+        return result;
+
+    ptr = 0;
+    dataStore[ptr++] = 0;       /* total len of factory cal */
+    dataStore[ptr++] = 0;
+    dataStore[ptr++] = 0;
+    dataStore[ptr++] = ML_INIT_CAL_LEN;
+    dataStore[ptr++] = 0;
+    dataStore[ptr++] = 5;       /* record type 5 - initial calibration */
+
+    tmp = temp_avg;             /* temperature */
+    if (tmp < 0) tmp += 2 << 16;
+    USHORT_TO_CHARS(&dataStore[ptr], tmp);
+    ptr += 2;
+
+    /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
+    lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+    lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+    lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+
+    lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+    lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+    lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */
+    if (lltmp < 0) lltmp += 1LL << 32;
+    UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp);
+    ptr += 4;
+
+    /* add a checksum for data */
+    chk = inv_checksum(
+        dataStore + INV_CAL_HDR_LEN,
+        ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
+    UINT_TO_CHARS(&dataStore[ptr], chk);
+    ptr += 4;
+
+    if (ptr != ML_INIT_CAL_LEN) {
+        MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
+                    ML_INIT_CAL_LEN, ptr);
+        return -1;
+    }
+
+    return result;
+}
+
+/**
+ *  @brief  The main test API. Runs the MPU Self Test and, if successful,
+ *          stores the encoded initial calibration data on the final storage
+ *          medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE
+ *          define in your mlsl implementation).
+ *
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  provide_result
+ *              If 1:
+ *              perform and analyze the offset, drive frequency, and noise
+ *              calculation and compare it against set threshouds. Report
+ *              also the final result using a bit-mask like error code as
+ *              described in the inv_test_gyro() function.
+ *              When 0:
+ *              skip the noise and drive frequency calculation and pass/fail
+ *              assessment. It simply calculates the gyro and accel biases.
+ *
+ *  @return 0 on success or a non-zero error code from the callees on error.
+ */
+inv_error_t inv_factory_calibrate(void *mlsl_handle,
+                                  uint_fast8_t provide_result)
+{
+    int result;
+
+    result = inv_mpu_test(mlsl_handle, provide_result);
+    if (provide_result) {
+        MPL_LOGI("\n");
+        if (result == 0) {
+            MPL_LOGI("Test : PASSED\n");
+        } else {
+            MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result);
+            return result; /* abort writing the calibration if the
+                              test is not successful */
+        }
+        MPL_LOGI("\n");
+    } else {
+        MPL_LOGI("\n");
+        if (result) {
+            LOG_RESULT_LOCATION(result);
+            return result;
+        }
+    }
+
+    result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN);
+    if (result) {
+        MPL_LOGI("Error : cannot write calibration on file - error %d\n",
+            result);
+        return result;
+    }
+
+    return INV_SUCCESS;
+}
+
+
+
+/* -----------------------------------------------------------------------
+    accel interface functions
+ -----------------------------------------------------------------------*/
+
+/**
+ *  @internal
+ *  @brief  Reads data for X, Y, and Z axis from the accelerometer device.
+ *          Used only if an accelerometer has been setup using the
+ *          inv_test_setup_accel() API.
+ *          Takes care of the accelerometer endianess according to how the
+ *          device has been described in the corresponding accelerometer driver
+ *          file.
+ *  @param  mlsl_handle
+ *              serial interface handle to allow serial communication with the
+ *              device, both gyro and accelerometer.
+ *  @param  slave
+ *              a pointer to the descriptor of the slave accelerometer device
+ *              in use. Contains the necessary information to operate, read,
+ *              and communicate with the accelerometer device of choice.
+ *              See the declaration of struct ext_slave_descr in mpu.h.
+ *  @param  pdata
+ *              a pointer to the platform info of the slave accelerometer
+ *              device in use. Describes how the device is oriented and
+ *              mounted on host platform's PCB.
+ *  @param  vals
+ *              output pointer to return the accelerometer's X, Y, and Z axis
+ *              sensor data collected.
+ *  @return 0 on success or a non-zero error code on error.
+ */
+static inv_error_t test_get_data(
+                    void *mlsl_handle,
+                    struct mldl_cfg *mputestCfgPtr,
+                    short *vals)
+{
+    inv_error_t result;
+    unsigned char data[20];
+    struct ext_slave_descr *slave = mputestCfgPtr->accel;
+#ifndef CONFIG_MPU_SENSORS_MPU3050
+    struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel;
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MPU3050
+    result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23,
+                             6, data);
+#else
+    result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg,
+                            slave->read_len, data);
+#endif
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    if (VERBOSE_OUT) {
+        MPL_LOGI("Accel         :        0x%02X%02X        0x%02X%02X        0x%02X%02X (raw)\n",
+            ACCEL_UNPACK(data));
+    }
+
+    if (CHECK_NACKS(data)) {
+        MPL_LOGI("Error fetching data from the accelerometer : "
+                 "all bytes read 0xff\n");
+        return INV_ERROR_SERIAL_READ;
+    }
+
+    if (slave->endian == EXT_SLAVE_BIG_ENDIAN) {
+        vals[0] = CHARS_TO_SHORT(&data[0]);
+        vals[1] = CHARS_TO_SHORT(&data[2]);
+        vals[2] = CHARS_TO_SHORT(&data[4]);
+    } else {
+        vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]);
+        vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]);
+        vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]);
+    }
+
+    if (VERBOSE_OUT) {
+        MPL_LOGI("Accel         : %+13d %+13d %+13d (LSB)\n",
+                 vals[0], vals[1], vals[2]);
+    }
+    return INV_SUCCESS;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ *  @}
+ */
+
diff --git a/60xx/mlsdk/mlutils/mputest.h b/60xx/mlsdk/mlutils/mputest.h
new file mode 100644
index 0000000..d3347c5
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/mlutils/slave.h b/60xx/mlsdk/mlutils/slave.h
new file mode 100644
index 0000000..45449f6
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/i2c.h b/60xx/mlsdk/platform/include/i2c.h
new file mode 100644
index 0000000..39dd255
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/linux/mpu.h b/60xx/mlsdk/platform/include/linux/mpu.h
new file mode 100644
index 0000000..04fa7b6
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/log.h b/60xx/mlsdk/platform/include/log.h
new file mode 100644
index 0000000..8485074
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/mlmath.h b/60xx/mlsdk/platform/include/mlmath.h
new file mode 100644
index 0000000..bf54870
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/mlos.h b/60xx/mlsdk/platform/include/mlos.h
new file mode 100644
index 0000000..0aeda96
--- /dev/null
+++ b/60xx/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;
+#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)
+	{
+		return inv_malloc((unsigned int)size);
+	}
+	static inline void *kzalloc(size_t size, unsigned int gfp_flags)
+	{
+		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/60xx/mlsdk/platform/include/mlsl.h b/60xx/mlsdk/platform/include/mlsl.h
new file mode 100644
index 0000000..535d117
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/mltypes.h b/60xx/mlsdk/platform/include/mltypes.h
new file mode 100644
index 0000000..90a126b
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/include/mpu3050.h b/60xx/mlsdk/platform/include/mpu3050.h
new file mode 100644
index 0000000..c363a00
--- /dev/null
+++ b/60xx/mlsdk/platform/include/mpu3050.h
@@ -0,0 +1,255 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+#ifndef __MPU_H_
+#error Do not include this file directly.  Include mpu.h instead.
+#endif
+
+#ifndef __MPU3050_H_
+#define __MPU3050_H_
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#endif
+
+#if !defined CONFIG_MPU_SENSORS_MPU3050
+#error MPU6000 build including MPU3050 header
+#endif
+
+#define MPU_NAME "mpu3050"
+#define DEFAULT_MPU_SLAVEADDR       0x68
+
+/*==== MPU REGISTER SET ====*/
+enum mpu_register {
+	MPUREG_WHO_AM_I = 0,	/* 00 0x00 */
+	MPUREG_PRODUCT_ID,	/* 01 0x01 */
+	MPUREG_02_RSVD,		/* 02 0x02 */
+	MPUREG_03_RSVD,		/* 03 0x03 */
+	MPUREG_04_RSVD,		/* 04 0x04 */
+	MPUREG_XG_OFFS_TC,	/* 05 0x05 */
+	MPUREG_06_RSVD,		/* 06 0x06 */
+	MPUREG_07_RSVD,		/* 07 0x07 */
+	MPUREG_YG_OFFS_TC,	/* 08 0x08 */
+	MPUREG_09_RSVD,		/* 09 0x09 */
+	MPUREG_0A_RSVD,		/* 10 0x0a */
+	MPUREG_ZG_OFFS_TC,	/* 11 0x0b */
+	MPUREG_X_OFFS_USRH,	/* 12 0x0c */
+	MPUREG_X_OFFS_USRL,	/* 13 0x0d */
+	MPUREG_Y_OFFS_USRH,	/* 14 0x0e */
+	MPUREG_Y_OFFS_USRL,	/* 15 0x0f */
+	MPUREG_Z_OFFS_USRH,	/* 16 0x10 */
+	MPUREG_Z_OFFS_USRL,	/* 17 0x11 */
+	MPUREG_FIFO_EN1,	/* 18 0x12 */
+	MPUREG_FIFO_EN2,	/* 19 0x13 */
+	MPUREG_AUX_SLV_ADDR,	/* 20 0x14 */
+	MPUREG_SMPLRT_DIV,	/* 21 0x15 */
+	MPUREG_DLPF_FS_SYNC,	/* 22 0x16 */
+	MPUREG_INT_CFG,		/* 23 0x17 */
+	MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */
+	MPUREG_19_RSVD,		/* 25 0x19 */
+	MPUREG_INT_STATUS,	/* 26 0x1a */
+	MPUREG_TEMP_OUT_H,	/* 27 0x1b */
+	MPUREG_TEMP_OUT_L,	/* 28 0x1c */
+	MPUREG_GYRO_XOUT_H,	/* 29 0x1d */
+	MPUREG_GYRO_XOUT_L,	/* 30 0x1e */
+	MPUREG_GYRO_YOUT_H,	/* 31 0x1f */
+	MPUREG_GYRO_YOUT_L,	/* 32 0x20 */
+	MPUREG_GYRO_ZOUT_H,	/* 33 0x21 */
+	MPUREG_GYRO_ZOUT_L,	/* 34 0x22 */
+	MPUREG_23_RSVD,		/* 35 0x23 */
+	MPUREG_24_RSVD,		/* 36 0x24 */
+	MPUREG_25_RSVD,		/* 37 0x25 */
+	MPUREG_26_RSVD,		/* 38 0x26 */
+	MPUREG_27_RSVD,		/* 39 0x27 */
+	MPUREG_28_RSVD,		/* 40 0x28 */
+	MPUREG_29_RSVD,		/* 41 0x29 */
+	MPUREG_2A_RSVD,		/* 42 0x2a */
+	MPUREG_2B_RSVD,		/* 43 0x2b */
+	MPUREG_2C_RSVD,		/* 44 0x2c */
+	MPUREG_2D_RSVD,		/* 45 0x2d */
+	MPUREG_2E_RSVD,		/* 46 0x2e */
+	MPUREG_2F_RSVD,		/* 47 0x2f */
+	MPUREG_30_RSVD,		/* 48 0x30 */
+	MPUREG_31_RSVD,		/* 49 0x31 */
+	MPUREG_32_RSVD,		/* 50 0x32 */
+	MPUREG_33_RSVD,		/* 51 0x33 */
+	MPUREG_34_RSVD,		/* 52 0x34 */
+	MPUREG_DMP_CFG_1,	/* 53 0x35 */
+	MPUREG_DMP_CFG_2,	/* 54 0x36 */
+	MPUREG_BANK_SEL,	/* 55 0x37 */
+	MPUREG_MEM_START_ADDR,	/* 56 0x38 */
+	MPUREG_MEM_R_W,		/* 57 0x39 */
+	MPUREG_FIFO_COUNTH,	/* 58 0x3a */
+	MPUREG_FIFO_COUNTL,	/* 59 0x3b */
+	MPUREG_FIFO_R_W,	/* 60 0x3c */
+	MPUREG_USER_CTRL,	/* 61 0x3d */
+	MPUREG_PWR_MGM,		/* 62 0x3e */
+	MPUREG_3F_RSVD,		/* 63 0x3f */
+	NUM_OF_MPU_REGISTERS	/* 64 0x40 */
+};
+
+/*==== BITS FOR MPU ====*/
+
+/*---- MPU 'FIFO_EN1' register (12) ----*/
+#define BIT_TEMP_OUT                0x80
+#define BIT_GYRO_XOUT               0x40
+#define BIT_GYRO_YOUT               0x20
+#define BIT_GYRO_ZOUT               0x10
+#define BIT_ACCEL_XOUT              0x08
+#define BIT_ACCEL_YOUT              0x04
+#define BIT_ACCEL_ZOUT              0x02
+#define BIT_AUX_1OUT                0x01
+/*---- MPU 'FIFO_EN2' register (13) ----*/
+#define BIT_AUX_2OUT                0x02
+#define BIT_AUX_3OUT                0x01
+/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/
+#define BITS_EXT_SYNC_NONE          0x00
+#define BITS_EXT_SYNC_TEMP          0x20
+#define BITS_EXT_SYNC_GYROX         0x40
+#define BITS_EXT_SYNC_GYROY         0x60
+#define BITS_EXT_SYNC_GYROZ         0x80
+#define BITS_EXT_SYNC_ACCELX        0xA0
+#define BITS_EXT_SYNC_ACCELY        0xC0
+#define BITS_EXT_SYNC_ACCELZ        0xE0
+#define BITS_EXT_SYNC_MASK          0xE0
+#define BITS_FS_250DPS              0x00
+#define BITS_FS_500DPS              0x08
+#define BITS_FS_1000DPS             0x10
+#define BITS_FS_2000DPS             0x18
+#define BITS_FS_MASK                0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+#define BITS_DLPF_CFG_MASK          0x07
+/*---- MPU 'INT_CFG' register (17) ----*/
+#define BIT_ACTL                    0x80
+#define BIT_ACTL_LOW                0x80
+#define BIT_ACTL_HIGH               0x00
+#define BIT_OPEN                    0x40
+#define BIT_OPEN_DRAIN              0x40
+#define BIT_PUSH_PULL               0x00
+#define BIT_LATCH_INT_EN            0x20
+#define BIT_INT_PULSE_WIDTH_50US    0x00
+#define BIT_INT_ANYRD_2CLEAR        0x10
+#define BIT_INT_STAT_READ_2CLEAR    0x00
+#define BIT_MPU_RDY_EN              0x04
+#define BIT_DMP_INT_EN              0x02
+#define BIT_RAW_RDY_EN              0x01
+/*---- MPU 'INT_STATUS' register (1A) ----*/
+#define BIT_INT_STATUS_FIFO_OVERLOW 0x80
+#define BIT_MPU_RDY                 0x04
+#define BIT_DMP_INT                 0x02
+#define BIT_RAW_RDY                 0x01
+/*---- MPU 'BANK_SEL' register (37) ----*/
+#define BIT_PRFTCH_EN               0x20
+#define BIT_CFG_USER_BANK           0x10
+#define BITS_MEM_SEL                0x0f
+/*---- MPU 'USER_CTRL' register (3D) ----*/
+#define BIT_DMP_EN                  0x80
+#define BIT_FIFO_EN                 0x40
+#define BIT_AUX_IF_EN               0x20
+#define BIT_AUX_RD_LENG             0x10
+#define BIT_AUX_IF_RST              0x08
+#define BIT_DMP_RST                 0x04
+#define BIT_FIFO_RST                0x02
+#define BIT_GYRO_RST                0x01
+/*---- MPU 'PWR_MGM' register (3E) ----*/
+#define BIT_H_RESET                 0x80
+#define BIT_SLEEP                   0x40
+#define BIT_STBY_XG                 0x20
+#define BIT_STBY_YG                 0x10
+#define BIT_STBY_ZG                 0x08
+#define BITS_CLKSEL                 0x07
+
+/*---- MPU Silicon Revision ----*/
+#define MPU_SILICON_REV_A4           1	/* MPU A4 Device */
+#define MPU_SILICON_REV_B1           2	/* MPU B1 Device */
+#define MPU_SILICON_REV_B4           3	/* MPU B4 Device */
+#define MPU_SILICON_REV_B6           4	/* MPU B6 Device */
+
+/*---- MPU Memory ----*/
+#define MPU_MEM_BANK_SIZE           (256)
+#define FIFO_HW_SIZE                (512)
+
+enum MPU_MEMORY_BANKS {
+	MPU_MEM_RAM_BANK_0 = 0,
+	MPU_MEM_RAM_BANK_1,
+	MPU_MEM_RAM_BANK_2,
+	MPU_MEM_RAM_BANK_3,
+	MPU_MEM_NUM_RAM_BANKS,
+	MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS,
+	/* This one is always last */
+	MPU_MEM_NUM_BANKS
+};
+
+/*---- structure containing control variables used by MLDL ----*/
+/*---- MPU clock source settings ----*/
+/*---- MPU filter selections ----*/
+enum mpu_filter {
+	MPU_FILTER_256HZ_NOLPF2 = 0,
+	MPU_FILTER_188HZ,
+	MPU_FILTER_98HZ,
+	MPU_FILTER_42HZ,
+	MPU_FILTER_20HZ,
+	MPU_FILTER_10HZ,
+	MPU_FILTER_5HZ,
+	MPU_FILTER_2100HZ_NOLPF,
+	NUM_MPU_FILTER
+};
+
+enum mpu_fullscale {
+	MPU_FS_250DPS = 0,
+	MPU_FS_500DPS,
+	MPU_FS_1000DPS,
+	MPU_FS_2000DPS,
+	NUM_MPU_FS
+};
+
+enum mpu_clock_sel {
+	MPU_CLK_SEL_INTERNAL = 0,
+	MPU_CLK_SEL_PLLGYROX,
+	MPU_CLK_SEL_PLLGYROY,
+	MPU_CLK_SEL_PLLGYROZ,
+	MPU_CLK_SEL_PLLEXT32K,
+	MPU_CLK_SEL_PLLEXT19M,
+	MPU_CLK_SEL_RESERVED,
+	MPU_CLK_SEL_STOP,
+	NUM_CLK_SEL
+};
+
+enum mpu_ext_sync {
+	MPU_EXT_SYNC_NONE = 0,
+	MPU_EXT_SYNC_TEMP,
+	MPU_EXT_SYNC_GYROX,
+	MPU_EXT_SYNC_GYROY,
+	MPU_EXT_SYNC_GYROZ,
+	MPU_EXT_SYNC_ACCELX,
+	MPU_EXT_SYNC_ACCELY,
+	MPU_EXT_SYNC_ACCELZ,
+	NUM_MPU_EXT_SYNC
+};
+
+#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \
+	((ext_sync << 5) | (full_scale << 3) | lpf)
+
+#endif				/* __MPU3050_H_ */
diff --git a/60xx/mlsdk/platform/include/stdint_invensense.h b/60xx/mlsdk/platform/include/stdint_invensense.h
new file mode 100644
index 0000000..d238813
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/kernel/mpuirq.h b/60xx/mlsdk/platform/linux/kernel/mpuirq.h
new file mode 100644
index 0000000..352170b
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/kernel/slaveirq.h b/60xx/mlsdk/platform/linux/kernel/slaveirq.h
new file mode 100644
index 0000000..beb352b
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/kernel/timerirq.h b/60xx/mlsdk/platform/linux/kernel/timerirq.h
new file mode 100644
index 0000000..dc3eea2
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/log_linux.c b/60xx/mlsdk/platform/linux/log_linux.c
new file mode 100644
index 0000000..8e75753
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/log_printf_linux.c b/60xx/mlsdk/platform/linux/log_printf_linux.c
new file mode 100644
index 0000000..744a223
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/mlos_linux.c b/60xx/mlsdk/platform/linux/mlos_linux.c
new file mode 100644
index 0000000..fd3b002
--- /dev/null
+++ b/60xx/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/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
new file mode 100644
index 0000000..1bd71de
--- /dev/null
+++ b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
@@ -0,0 +1,497 @@
+/*
+ $License:
+   Copyright 2011 InvenSense, Inc.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+  $
+ */
+
+/******************************************************************************
+ * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
+ *****************************************************************************/
+
+/**
+ *  @defgroup MLSL (Motion Library - Serial Layer)
+ *  @brief  The Motion Library System Layer provides the Motion Library the
+ *          interface to the system functions.
+ *
+ *  @{
+ *      @file   mlsl_linux_mpu.c
+ *      @brief  The Motion Library System Layer.
+ *
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <string.h>
+#include <signal.h>
+#include <time.h>
+
+#include "mpu.h"
+#if defined CONFIG_MPU_SENSORS_MPU6050A2
+#    include "mpu6050a2.h"
+#elif defined CONFIG_MPU_SENSORS_MPU6050B1
+#    include "mpu6050b1.h"
+#elif defined CONFIG_MPU_SENSORS_MPU3050
+#  include "mpu3050.h"
+#else
+#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
+#endif
+
+#include "mlsl.h"
+#include "mlos.h"
+#include "mlmath.h"
+#include "mlinclude.h"
+
+#define MLCAL_ID      (0x0A0B0C0DL)
+#define MLCAL_FILE    "/data/cal.bin"
+#define MLCFG_ID      (0x01020304L)
+#define MLCFG_FILE    "/data/cfg.bin"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mlsl"
+
+#ifndef I2CDEV
+#define I2CDEV "/dev/mpu"
+#endif
+
+#define SERIAL_FULL_DEBUG (0)
+
+/* --------------- */
+/* - Prototypes. - */
+/* --------------- */
+
+/* ----------------------- */
+/* -  Function Pointers. - */
+/* ----------------------- */
+
+/* --------------------------- */
+/* - Global and Static vars. - */
+/* --------------------------- */
+
+/* ---------------- */
+/* - Definitions. - */
+/* ---------------- */
+
+inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len)
+{
+    FILE *fp;
+    int bytesRead;
+
+    fp = fopen(MLCFG_FILE, "rb");
+    if (fp == NULL) {
+        MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesRead = fread(cfg, 1, len, fp);
+    if (bytesRead != len) {
+        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
+                 bytesRead, len);
+        return INV_ERROR_FILE_READ;
+    }
+    fclose(fp);
+
+    if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3])
+        != MLCFG_ID) {
+        return INV_ERROR_ASSERTION_FAILURE;
+    }
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len)
+{
+    FILE *fp;
+    int bytesWritten;
+    unsigned char cfgId[4];
+
+    fp = fopen(MLCFG_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+
+    cfgId[0] = (unsigned char)(MLCFG_ID >> 24);
+    cfgId[1] = (unsigned char)(MLCFG_ID >> 16);
+    cfgId[2] = (unsigned char)(MLCFG_ID >> 8);
+    cfgId[3] = (unsigned char)(MLCFG_ID);
+    bytesWritten = fwrite(cfgId, 1, 4, fp);
+    if (bytesWritten != 4) {
+        MPL_LOGE("CFG ID could not be written on file\n");
+        return INV_ERROR_FILE_WRITE;
+    }
+
+    bytesWritten = fwrite(cfg, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes write (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        return INV_ERROR_FILE_WRITE;
+    }
+
+    fclose(fp);
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len)
+{
+    FILE *fp;
+    int bytesRead;
+    inv_error_t result = INV_SUCCESS;
+
+    fp = fopen(MLCAL_FILE,"rb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesRead = fread(cal, 1, len, fp);
+    if (bytesRead != len) {
+        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
+                 bytesRead, len);
+        result = INV_ERROR_FILE_READ;
+        goto read_cal_end;
+    }
+
+    /* MLCAL_ID not used
+    if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3])
+        != MLCAL_ID) {
+        result = INV_ERROR_ASSERTION_FAILURE;
+        goto read_cal_end;
+    }
+    */
+read_cal_end:
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len)
+{
+    FILE *fp;
+    int bytesWritten;
+    inv_error_t result = INV_SUCCESS;
+
+    fp = fopen(MLCAL_FILE,"wb");
+    if (fp == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+    bytesWritten = fwrite(cal, 1, len, fp);
+    if (bytesWritten != len) {
+        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
+                 bytesWritten, len);
+        result = INV_ERROR_FILE_WRITE;
+    }
+    fclose(fp);
+    return result;
+}
+
+inv_error_t inv_serial_get_cal_length(unsigned int *len)
+{
+    FILE *calFile;
+    *len = 0;
+
+    calFile = fopen(MLCAL_FILE, "rb");
+    if (calFile == NULL) {
+        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
+        return INV_ERROR_FILE_OPEN;
+    }
+
+    *len += (unsigned char)fgetc(calFile) << 24;
+    *len += (unsigned char)fgetc(calFile) << 16;
+    *len += (unsigned char)fgetc(calFile) << 8;
+    *len += (unsigned char)fgetc(calFile);
+
+    fclose(calFile);
+
+    if (*len <= 0)
+        return INV_ERROR_FILE_READ;
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_open(char const *port, void **sl_handle)
+{
+    INVENSENSE_FUNC_START;
+
+    if (NULL == port) {
+        port = I2CDEV;
+    }
+    *sl_handle = (void*)(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)
+{
+    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+inv_error_t inv_serial_single_write(void *sl_handle,
+                               unsigned char slaveAddr,
+                               unsigned char registerAddr,
+                               unsigned char data)
+{
+    unsigned char buf[2];
+    buf[0] = registerAddr;
+    buf[1] = data;
+    return inv_serial_write(sl_handle, slaveAddr, 2, buf);
+}
+
+inv_error_t inv_serial_write(void *sl_handle,
+                         unsigned char slaveAddr,
+                         unsigned short length,
+                         unsigned char const *data)
+{
+    INVENSENSE_FUNC_START;
+    struct mpu_read_write msg;
+    inv_error_t result;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    msg.address = 0; /* not used */
+    msg.length  = length;
+    msg.data    = (unsigned char*)data;
+
+    if ((result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C Write Success %02x %02x: %s \n",
+                 data[0], length, data_buff);
+    }
+
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_read(void *sl_handle,
+                        unsigned char  slaveAddr,
+                        unsigned char  registerAddr,
+                        unsigned short length,
+                        unsigned char  *data)
+{
+    INVENSENSE_FUNC_START;
+    int result = INV_SUCCESS;
+    struct mpu_read_write msg;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    msg.address = registerAddr;
+    msg.length  = length;
+    msg.data    = data;
+
+    result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C Read  Success %02x %02x: %s \n",
+                  registerAddr, length, data_buff);
+    }
+
+    return (inv_error_t) result;
+}
+
+inv_error_t inv_serial_write_mem(void *sl_handle,
+                            unsigned char mpu_addr,
+                            unsigned short memAddr,
+                            unsigned short length,
+                            const unsigned char *data)
+{
+    INVENSENSE_FUNC_START;
+    struct mpu_read_write msg;
+    int result;
+
+    msg.address = memAddr;
+    msg.length  = length;
+    msg.data    = (unsigned char *)data;
+
+    result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n",
+                 memAddr, length, data_buff);
+    }
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_read_mem(void *sl_handle,
+                           unsigned char  mpu_addr,
+                           unsigned short memAddr,
+                           unsigned short length,
+                           unsigned char  *data)
+{
+    INVENSENSE_FUNC_START;
+    struct mpu_read_write msg;
+    int result;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    msg.address = memAddr;
+    msg.length  = length;
+    msg.data    = data;
+
+    result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n",
+                 memAddr, length, data_buff);
+    }
+    return INV_SUCCESS;
+}
+
+inv_error_t inv_serial_write_fifo(void *sl_handle,
+                             unsigned char mpu_addr,
+                             unsigned short length,
+                             const unsigned char *data)
+{
+    INVENSENSE_FUNC_START;
+    struct mpu_read_write msg;
+    int result;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    msg.address = 0; /* Not used */
+    msg.length  = length;
+    msg.data    = (unsigned char *)data;
+
+    result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C Write Success %02x %02x: %s\n",
+                 MPUREG_FIFO_R_W, length, data_buff);
+    }
+    return (inv_error_t) result;
+}
+
+inv_error_t inv_serial_read_fifo(void *sl_handle,
+                            unsigned char  mpu_addr,
+                            unsigned short length,
+                            unsigned char  *data)
+{
+    INVENSENSE_FUNC_START;
+    struct mpu_read_write msg;
+    int result;
+
+    if (NULL == data) {
+        return INV_ERROR_INVALID_PARAMETER;
+    }
+
+    msg.address = MPUREG_FIFO_R_W; /* Not used */
+    msg.length  = length;
+    msg.data    = data;
+
+    result = ioctl((int)(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));
+        }
+        MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
+                 MPUREG_FIFO_R_W, length, data_buff);
+    }
+    return INV_SUCCESS;
+}
+
+/**
+ *  @}
+ */
+
+
diff --git a/Android.mk b/Android.mk
index ff26264..65c9e4f 100644
--- a/Android.mk
+++ b/Android.mk
@@ -2,7 +2,14 @@
 ifneq ($(filter hammerhead, $(TARGET_DEVICE)),)
 # hammerhead expects 65xx sensors.
 include $(call all-named-subdir-makefiles,65xx)
-else ifneq ($(filter guppy dory, $(TARGET_DEVICE)),)
+else
+ifneq ($(filter guppy dory, $(TARGET_DEVICE)),)
 # dory and guppy expect 6515 sensors.
 include $(call all-named-subdir-makefiles,6515)
+else
+ifneq ($(filter manta grouper tuna mako, $(TARGET_DEVICE)),)
+# manta, grouper, tuna, and mako expect 60xx sensors.
+include $(call all-named-subdir-makefiles,60xx)
+endif
+endif
 endif